├── .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_to_point_control │ ├── NLinkArm.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 ├── LICENSE ├── Localization ├── Kalmanfilter_basics.ipynb ├── Kalmanfilter_basics_2.ipynb ├── bayes_filter.png ├── extended_kalman_filter │ ├── ekf.png │ ├── extended_kalman_filter.py │ └── extended_kalman_filter_localization.ipynb ├── histogram_filter │ └── histogram_filter.py ├── particle_filter │ └── particle_filter.py └── unscented_kalman_filter │ └── unscented_kalman_filter.py ├── Mapping ├── circle_fitting │ └── circle_fitting.py ├── gaussian_grid_map │ └── gaussian_grid_map.py ├── kmeans_clustering │ └── kmeans_clustering.py ├── 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 ├── 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 │ └── cubic_spline_planner.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 │ ├── cubic_spline_planner.py │ └── frenet_optimal_trajectory.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.py ├── RRT │ ├── figure_1.png │ ├── rrt_with_pathsmoothing.py │ └── simple_rrt.py ├── RRTDubins │ ├── __init__.py │ └── rrt_dubins.py ├── RRTStarDubins │ ├── dubins_path_planning.py │ └── rrt_star_dubins.py ├── RRTStarReedsShepp │ ├── figure_1.png │ └── rrt_star_reeds_shepp.py ├── RRTstar │ ├── figure_1.png │ ├── rrt_star.ipynb │ └── rrt_star.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 ├── VoronoiRoadMap │ └── voronoi_road_map.py └── __init__.py ├── PathTracking ├── .gitignore ├── 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 │ └── ekf_slam.py ├── FastSLAM1 │ ├── FastSLAM1.ipynb │ ├── animation.png │ └── fast_slam1.py ├── FastSLAM2 │ └── fast_slam2.py ├── GraphBasedSLAM │ └── graph_based_slam.py ├── __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 ├── requirements.txt ├── 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_histogram_filter.py ├── test_informed_rrt_star.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_voronoi_path_planner.py └── users_comments.md /.gitignore: -------------------------------------------------------------------------------- 1 | *.csv 2 | *.gif 3 | 4 | # Byte-compiled / optimized / DLL files 5 | __pycache__/ 6 | *.py[cod] 7 | *$py.class 8 | _build/ 9 | .idea/ 10 | 11 | # C extensions 12 | *.so 13 | 14 | # Distribution / packaging 15 | .Python 16 | env/ 17 | build/ 18 | develop-eggs/ 19 | dist/ 20 | downloads/ 21 | eggs/ 22 | .eggs/ 23 | lib/ 24 | lib64/ 25 | parts/ 26 | sdist/ 27 | var/ 28 | *.egg-info/ 29 | .installed.cfg 30 | *.egg 31 | 32 | .DS_Store 33 | 34 | # PyInstaller 35 | # Usually these files are written by a python script from a template 36 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 37 | *.manifest 38 | *.spec 39 | 40 | # Installer logs 41 | pip-log.txt 42 | pip-delete-this-directory.txt 43 | 44 | # Unit test / coverage reports 45 | htmlcov/ 46 | .tox/ 47 | .coverage 48 | .coverage.* 49 | .cache 50 | nosetests.xml 51 | coverage.xml 52 | *,cover 53 | .hypothesis/ 54 | 55 | # Translations 56 | *.mo 57 | *.pot 58 | 59 | # Django stuff: 60 | *.log 61 | 62 | # Sphinx documentation 63 | docs/_build/ 64 | 65 | # PyBuilder 66 | target/ 67 | 68 | #Ipython Notebook 69 | .ipynb_checkpoints 70 | -------------------------------------------------------------------------------- /.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 | #- os: osx 7 | #python: "3.6-dev" 8 | 9 | python: 10 | - 3.6 11 | 12 | before_install: 13 | - sudo apt-get update 14 | - wget https://repo.continuum.io/miniconda/Miniconda3-latest-Linux-x86_64.sh -O miniconda.sh 15 | - chmod +x miniconda.sh 16 | - bash miniconda.sh -b -p $HOME/miniconda 17 | - export PATH="$HOME/miniconda/bin:$PATH" 18 | - hash -r 19 | - conda config --set always_yes yes --set changeps1 no 20 | - conda update -q conda 21 | # Useful for debugging any issues with conda 22 | - conda info -a 23 | - conda install python==3.6.8 24 | #- conda env create -f ./environment.yml 25 | #- activate python_robotics 26 | 27 | 28 | install: 29 | - conda install numpy 30 | - conda install scipy 31 | - conda install matplotlib 32 | - conda install pandas 33 | - conda install -c conda-forge lapack 34 | - conda install -c cvxgrp cvxpy 35 | - conda install coveralls 36 | 37 | script: 38 | - python --version 39 | - ./runtests.sh 40 | 41 | after_success: 42 | - coveralls 43 | -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/Quadrotor.py: -------------------------------------------------------------------------------- 1 | """ 2 | Class for plotting a quadrotor 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | from math import cos, sin 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | 12 | class Quadrotor(): 13 | def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True): 14 | self.p1 = np.array([size / 2, 0, 0, 1]).T 15 | self.p2 = np.array([-size / 2, 0, 0, 1]).T 16 | self.p3 = np.array([0, size / 2, 0, 1]).T 17 | self.p4 = np.array([0, -size / 2, 0, 1]).T 18 | 19 | self.x_data = [] 20 | self.y_data = [] 21 | self.z_data = [] 22 | self.show_animation = show_animation 23 | 24 | if self.show_animation: 25 | plt.ion() 26 | fig = plt.figure() 27 | self.ax = fig.add_subplot(111, projection='3d') 28 | 29 | self.update_pose(x, y, z, roll, pitch, yaw) 30 | 31 | def update_pose(self, x, y, z, roll, pitch, yaw): 32 | self.x = x 33 | self.y = y 34 | self.z = z 35 | self.roll = roll 36 | self.pitch = pitch 37 | self.yaw = yaw 38 | self.x_data.append(x) 39 | self.y_data.append(y) 40 | self.z_data.append(z) 41 | 42 | if self.show_animation: 43 | self.plot() 44 | 45 | def transformation_matrix(self): 46 | x = self.x 47 | y = self.y 48 | z = self.z 49 | roll = self.roll 50 | pitch = self.pitch 51 | yaw = self.yaw 52 | return np.array( 53 | [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x], 54 | [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) 55 | * sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y], 56 | [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z] 57 | ]) 58 | 59 | def plot(self): # pragma: no cover 60 | T = self.transformation_matrix() 61 | 62 | p1_t = np.matmul(T, self.p1) 63 | p2_t = np.matmul(T, self.p2) 64 | p3_t = np.matmul(T, self.p3) 65 | p4_t = np.matmul(T, self.p4) 66 | 67 | plt.cla() 68 | 69 | self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]], 70 | [p1_t[1], p2_t[1], p3_t[1], p4_t[1]], 71 | [p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.') 72 | 73 | self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]], 74 | [p1_t[2], p2_t[2]], 'r-') 75 | self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]], 76 | [p3_t[2], p4_t[2]], 'r-') 77 | 78 | self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:') 79 | 80 | plt.xlim(-5, 5) 81 | plt.ylim(-5, 5) 82 | self.ax.set_zlim(0, 10) 83 | 84 | plt.pause(0.001) -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/TrajectoryGenerator.py: -------------------------------------------------------------------------------- 1 | """ 2 | Generates a quintic polynomial trajectory. 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | import numpy as np 8 | 9 | class TrajectoryGenerator(): 10 | def __init__(self, start_pos, des_pos, T, start_vel=[0,0,0], des_vel=[0,0,0], start_acc=[0,0,0], des_acc=[0,0,0]): 11 | self.start_x = start_pos[0] 12 | self.start_y = start_pos[1] 13 | self.start_z = start_pos[2] 14 | 15 | self.des_x = des_pos[0] 16 | self.des_y = des_pos[1] 17 | self.des_z = des_pos[2] 18 | 19 | self.start_x_vel = start_vel[0] 20 | self.start_y_vel = start_vel[1] 21 | self.start_z_vel = start_vel[2] 22 | 23 | self.des_x_vel = des_vel[0] 24 | self.des_y_vel = des_vel[1] 25 | self.des_z_vel = des_vel[2] 26 | 27 | self.start_x_acc = start_acc[0] 28 | self.start_y_acc = start_acc[1] 29 | self.start_z_acc = start_acc[2] 30 | 31 | self.des_x_acc = des_acc[0] 32 | self.des_y_acc = des_acc[1] 33 | self.des_z_acc = des_acc[2] 34 | 35 | self.T = T 36 | 37 | def solve(self): 38 | A = np.array( 39 | [[0, 0, 0, 0, 0, 1], 40 | [self.T**5, self.T**4, self.T**3, self.T**2, self.T, 1], 41 | [0, 0, 0, 0, 1, 0], 42 | [5*self.T**4, 4*self.T**3, 3*self.T**2, 2*self.T, 1, 0], 43 | [0, 0, 0, 2, 0, 0], 44 | [20*self.T**3, 12*self.T**2, 6*self.T, 2, 0, 0] 45 | ]) 46 | 47 | b_x = np.array( 48 | [[self.start_x], 49 | [self.des_x], 50 | [self.start_x_vel], 51 | [self.des_x_vel], 52 | [self.start_x_acc], 53 | [self.des_x_acc] 54 | ]) 55 | 56 | b_y = np.array( 57 | [[self.start_y], 58 | [self.des_y], 59 | [self.start_y_vel], 60 | [self.des_y_vel], 61 | [self.start_y_acc], 62 | [self.des_y_acc] 63 | ]) 64 | 65 | b_z = np.array( 66 | [[self.start_z], 67 | [self.des_z], 68 | [self.start_z_vel], 69 | [self.des_z_vel], 70 | [self.start_z_acc], 71 | [self.des_z_acc] 72 | ]) 73 | 74 | self.x_c = np.linalg.solve(A, b_x) 75 | self.y_c = np.linalg.solve(A, b_y) 76 | self.z_c = np.linalg.solve(A, b_z) -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py: -------------------------------------------------------------------------------- 1 | """ 2 | Simulate a quadrotor following a 3D trajectory 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | from math import cos, sin 8 | import numpy as np 9 | from Quadrotor import Quadrotor 10 | from TrajectoryGenerator import TrajectoryGenerator 11 | from mpl_toolkits.mplot3d import Axes3D 12 | 13 | show_animation = True 14 | 15 | # Simulation parameters 16 | g = 9.81 17 | m = 0.2 18 | Ixx = 1 19 | Iyy = 1 20 | Izz = 1 21 | T = 5 22 | 23 | # Proportional coefficients 24 | Kp_x = 1 25 | Kp_y = 1 26 | Kp_z = 1 27 | Kp_roll = 25 28 | Kp_pitch = 25 29 | Kp_yaw = 25 30 | 31 | # Derivative coefficients 32 | Kd_x = 10 33 | Kd_y = 10 34 | Kd_z = 1 35 | 36 | 37 | def quad_sim(x_c, y_c, z_c): 38 | """ 39 | Calculates the necessary thrust and torques for the quadrotor to 40 | follow the trajectory described by the sets of coefficients 41 | x_c, y_c, and z_c. 42 | """ 43 | x_pos = -5 44 | y_pos = -5 45 | z_pos = 5 46 | x_vel = 0 47 | y_vel = 0 48 | z_vel = 0 49 | x_acc = 0 50 | y_acc = 0 51 | z_acc = 0 52 | roll = 0 53 | pitch = 0 54 | yaw = 0 55 | roll_vel = 0 56 | pitch_vel = 0 57 | yaw_vel = 0 58 | 59 | des_yaw = 0 60 | 61 | dt = 0.1 62 | t = 0 63 | 64 | q = Quadrotor(x=x_pos, y=y_pos, z=z_pos, roll=roll, 65 | pitch=pitch, yaw=yaw, size=1, show_animation=show_animation) 66 | 67 | i = 0 68 | n_run = 8 69 | irun = 0 70 | 71 | while True: 72 | while t <= T: 73 | # des_x_pos = calculate_position(x_c[i], t) 74 | # des_y_pos = calculate_position(y_c[i], t) 75 | des_z_pos = calculate_position(z_c[i], t) 76 | # des_x_vel = calculate_velocity(x_c[i], t) 77 | # des_y_vel = calculate_velocity(y_c[i], t) 78 | des_z_vel = calculate_velocity(z_c[i], t) 79 | des_x_acc = calculate_acceleration(x_c[i], t) 80 | des_y_acc = calculate_acceleration(y_c[i], t) 81 | des_z_acc = calculate_acceleration(z_c[i], t) 82 | 83 | thrust = m * (g + des_z_acc + Kp_z * (des_z_pos - 84 | z_pos) + Kd_z * (des_z_vel - z_vel)) 85 | 86 | roll_torque = Kp_roll * \ 87 | (((des_x_acc * sin(des_yaw) - des_y_acc * cos(des_yaw)) / g) - roll) 88 | pitch_torque = Kp_pitch * \ 89 | (((des_x_acc * cos(des_yaw) - des_y_acc * sin(des_yaw)) / g) - pitch) 90 | yaw_torque = Kp_yaw * (des_yaw - yaw) 91 | 92 | roll_vel += roll_torque * dt / Ixx 93 | pitch_vel += pitch_torque * dt / Iyy 94 | yaw_vel += yaw_torque * dt / Izz 95 | 96 | roll += roll_vel * dt 97 | pitch += pitch_vel * dt 98 | yaw += yaw_vel * dt 99 | 100 | R = rotation_matrix(roll, pitch, yaw) 101 | acc = (np.matmul(R, np.array( 102 | [0, 0, thrust]).T) - np.array([0, 0, m * g]).T) / m 103 | x_acc = acc[0] 104 | y_acc = acc[1] 105 | z_acc = acc[2] 106 | x_vel += x_acc * dt 107 | y_vel += y_acc * dt 108 | z_vel += z_acc * dt 109 | x_pos += x_vel * dt 110 | y_pos += y_vel * dt 111 | z_pos += z_vel * dt 112 | 113 | q.update_pose(x_pos, y_pos, z_pos, roll, pitch, yaw) 114 | 115 | t += dt 116 | 117 | t = 0 118 | i = (i + 1) % 4 119 | irun += 1 120 | if irun >= n_run: 121 | break 122 | 123 | print("Done") 124 | 125 | 126 | def calculate_position(c, t): 127 | """ 128 | Calculates a position given a set of quintic coefficients and a time. 129 | 130 | Args 131 | c: List of coefficients generated by a quintic polynomial 132 | trajectory generator. 133 | t: Time at which to calculate the position 134 | 135 | Returns 136 | Position 137 | """ 138 | return c[0] * t**5 + c[1] * t**4 + c[2] * t**3 + c[3] * t**2 + c[4] * t + c[5] 139 | 140 | 141 | def calculate_velocity(c, t): 142 | """ 143 | Calculates a velocity given a set of quintic coefficients and a time. 144 | 145 | Args 146 | c: List of coefficients generated by a quintic polynomial 147 | trajectory generator. 148 | t: Time at which to calculate the velocity 149 | 150 | Returns 151 | Velocity 152 | """ 153 | return 5 * c[0] * t**4 + 4 * c[1] * t**3 + 3 * c[2] * t**2 + 2 * c[3] * t + c[4] 154 | 155 | 156 | def calculate_acceleration(c, t): 157 | """ 158 | Calculates an acceleration given a set of quintic coefficients and a time. 159 | 160 | Args 161 | c: List of coefficients generated by a quintic polynomial 162 | trajectory generator. 163 | t: Time at which to calculate the acceleration 164 | 165 | Returns 166 | Acceleration 167 | """ 168 | return 20 * c[0] * t**3 + 12 * c[1] * t**2 + 6 * c[2] * t + 2 * c[3] 169 | 170 | 171 | def rotation_matrix(roll, pitch, yaw): 172 | """ 173 | Calculates the ZYX rotation matrix. 174 | 175 | Args 176 | Roll: Angular position about the x-axis in radians. 177 | Pitch: Angular position about the y-axis in radians. 178 | Yaw: Angular position about the z-axis in radians. 179 | 180 | Returns 181 | 3x3 rotation matrix as NumPy array 182 | """ 183 | return np.array( 184 | [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll)], 185 | [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) * 186 | sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll)], 187 | [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw)] 188 | ]) 189 | 190 | 191 | def main(): 192 | """ 193 | Calculates the x, y, z coefficients for the four segments 194 | of the trajectory 195 | """ 196 | x_coeffs = [[], [], [], []] 197 | y_coeffs = [[], [], [], []] 198 | z_coeffs = [[], [], [], []] 199 | waypoints = [[-5, -5, 5], [5, -5, 5], [5, 5, 5], [-5, 5, 5]] 200 | 201 | for i in range(4): 202 | traj = TrajectoryGenerator(waypoints[i], waypoints[(i + 1) % 4], T) 203 | traj.solve() 204 | x_coeffs[i] = traj.x_c 205 | y_coeffs[i] = traj.y_c 206 | z_coeffs[i] = traj.z_c 207 | 208 | quad_sim(x_coeffs, y_coeffs, z_coeffs) 209 | 210 | 211 | if __name__ == "__main__": 212 | main() -------------------------------------------------------------------------------- /AerialNavigation/rocket_powered_landing/figure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/AerialNavigation/rocket_powered_landing/figure.png -------------------------------------------------------------------------------- /ArmNavigation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/ArmNavigation/__init__.py -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py: -------------------------------------------------------------------------------- 1 | """ 2 | Class for controlling and plotting an arm with an arbitrary number of links. 3 | 4 | Author: Daniel Ingram 5 | """ 6 | 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | 11 | class NLinkArm(object): 12 | def __init__(self, link_lengths, joint_angles, goal, show_animation): 13 | self.show_animation = show_animation 14 | self.n_links = len(link_lengths) 15 | if self.n_links != len(joint_angles): 16 | raise ValueError() 17 | 18 | self.link_lengths = np.array(link_lengths) 19 | self.joint_angles = np.array(joint_angles) 20 | self.points = [[0, 0] for _ in range(self.n_links + 1)] 21 | 22 | self.lim = sum(link_lengths) 23 | self.goal = np.array(goal).T 24 | 25 | if show_animation: # pragma: no cover 26 | self.fig = plt.figure() 27 | self.fig.canvas.mpl_connect('button_press_event', self.click) 28 | 29 | plt.ion() 30 | plt.show() 31 | 32 | self.update_points() 33 | 34 | def update_joints(self, joint_angles): 35 | self.joint_angles = joint_angles 36 | 37 | self.update_points() 38 | 39 | def update_points(self): 40 | for i in range(1, self.n_links + 1): 41 | self.points[i][0] = self.points[i - 1][0] + \ 42 | self.link_lengths[i - 1] * \ 43 | np.cos(np.sum(self.joint_angles[:i])) 44 | self.points[i][1] = self.points[i - 1][1] + \ 45 | self.link_lengths[i - 1] * \ 46 | np.sin(np.sum(self.joint_angles[:i])) 47 | 48 | self.end_effector = np.array(self.points[self.n_links]).T 49 | if self.show_animation: # pragma: no cover 50 | self.plot() 51 | 52 | def plot(self): # pragma: no cover 53 | plt.cla() 54 | 55 | for i in range(self.n_links + 1): 56 | if i is not self.n_links: 57 | plt.plot([self.points[i][0], self.points[i + 1][0]], 58 | [self.points[i][1], self.points[i + 1][1]], 'r-') 59 | plt.plot(self.points[i][0], self.points[i][1], 'ko') 60 | 61 | plt.plot(self.goal[0], self.goal[1], 'gx') 62 | 63 | plt.plot([self.end_effector[0], self.goal[0]], [ 64 | self.end_effector[1], self.goal[1]], 'g--') 65 | 66 | plt.xlim([-self.lim, self.lim]) 67 | plt.ylim([-self.lim, self.lim]) 68 | plt.draw() 69 | plt.pause(0.0001) 70 | 71 | def click(self, event): 72 | self.goal = np.array([event.xdata, event.ydata]).T 73 | self.plot() 74 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Inverse kinematics for an n-link arm using the Jacobian inverse method 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | Atsushi Sakai (@Atsushi_twi) 6 | """ 7 | import numpy as np 8 | 9 | from NLinkArm import NLinkArm 10 | 11 | # Simulation parameters 12 | Kp = 2 13 | dt = 0.1 14 | N_LINKS = 10 15 | N_ITERATIONS = 10000 16 | 17 | # States 18 | WAIT_FOR_NEW_GOAL = 1 19 | MOVING_TO_GOAL = 2 20 | 21 | show_animation = True 22 | 23 | 24 | def main(): # pragma: no cover 25 | """ 26 | Creates an arm using the NLinkArm class and uses its inverse kinematics 27 | to move it to the desired position. 28 | """ 29 | link_lengths = [1] * N_LINKS 30 | joint_angles = np.array([0] * N_LINKS) 31 | goal_pos = [N_LINKS, 0] 32 | arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation) 33 | state = WAIT_FOR_NEW_GOAL 34 | solution_found = False 35 | while True: 36 | old_goal = np.array(goal_pos) 37 | goal_pos = np.array(arm.goal) 38 | end_effector = arm.end_effector 39 | errors, distance = distance_to_goal(end_effector, goal_pos) 40 | 41 | # State machine to allow changing of goal before current goal has been reached 42 | if state is WAIT_FOR_NEW_GOAL: 43 | if distance > 0.1 and not solution_found: 44 | joint_goal_angles, solution_found = inverse_kinematics( 45 | link_lengths, joint_angles, goal_pos) 46 | if not solution_found: 47 | print("Solution could not be found.") 48 | state = WAIT_FOR_NEW_GOAL 49 | arm.goal = end_effector 50 | elif solution_found: 51 | state = MOVING_TO_GOAL 52 | elif state is MOVING_TO_GOAL: 53 | if distance > 0.1 and all(old_goal == goal_pos): 54 | joint_angles = joint_angles + Kp * \ 55 | ang_diff(joint_goal_angles, joint_angles) * dt 56 | else: 57 | state = WAIT_FOR_NEW_GOAL 58 | solution_found = False 59 | 60 | arm.update_joints(joint_angles) 61 | 62 | 63 | def inverse_kinematics(link_lengths, joint_angles, goal_pos): 64 | """ 65 | Calculates the inverse kinematics using the Jacobian inverse method. 66 | """ 67 | for iteration in range(N_ITERATIONS): 68 | current_pos = forward_kinematics(link_lengths, joint_angles) 69 | errors, distance = distance_to_goal(current_pos, goal_pos) 70 | if distance < 0.1: 71 | print("Solution found in %d iterations." % iteration) 72 | return joint_angles, True 73 | J = jacobian_inverse(link_lengths, joint_angles) 74 | joint_angles = joint_angles + np.matmul(J, errors) 75 | return joint_angles, False 76 | 77 | 78 | def get_random_goal(): 79 | from random import random 80 | SAREA = 15.0 81 | return [SAREA * random() - SAREA / 2.0, 82 | SAREA * random() - SAREA / 2.0] 83 | 84 | 85 | def animation(): 86 | link_lengths = [1] * N_LINKS 87 | joint_angles = np.array([0] * N_LINKS) 88 | goal_pos = get_random_goal() 89 | arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation) 90 | state = WAIT_FOR_NEW_GOAL 91 | solution_found = False 92 | 93 | i_goal = 0 94 | while True: 95 | old_goal = np.array(goal_pos) 96 | goal_pos = np.array(arm.goal) 97 | end_effector = arm.end_effector 98 | errors, distance = distance_to_goal(end_effector, goal_pos) 99 | 100 | # State machine to allow changing of goal before current goal has been reached 101 | if state is WAIT_FOR_NEW_GOAL: 102 | 103 | if distance > 0.1 and not solution_found: 104 | joint_goal_angles, solution_found = inverse_kinematics( 105 | link_lengths, joint_angles, goal_pos) 106 | if not solution_found: 107 | print("Solution could not be found.") 108 | state = WAIT_FOR_NEW_GOAL 109 | arm.goal = get_random_goal() 110 | elif solution_found: 111 | state = MOVING_TO_GOAL 112 | elif state is MOVING_TO_GOAL: 113 | if distance > 0.1 and all(old_goal == goal_pos): 114 | joint_angles = joint_angles + Kp * \ 115 | ang_diff(joint_goal_angles, joint_angles) * dt 116 | else: 117 | state = WAIT_FOR_NEW_GOAL 118 | solution_found = False 119 | arm.goal = get_random_goal() 120 | i_goal += 1 121 | 122 | if i_goal >= 5: 123 | break 124 | 125 | arm.update_joints(joint_angles) 126 | 127 | 128 | def forward_kinematics(link_lengths, joint_angles): 129 | x = y = 0 130 | for i in range(1, N_LINKS + 1): 131 | x += link_lengths[i - 1] * np.cos(np.sum(joint_angles[:i])) 132 | y += link_lengths[i - 1] * np.sin(np.sum(joint_angles[:i])) 133 | return np.array([x, y]).T 134 | 135 | 136 | def jacobian_inverse(link_lengths, joint_angles): 137 | J = np.zeros((2, N_LINKS)) 138 | for i in range(N_LINKS): 139 | J[0, i] = 0 140 | J[1, i] = 0 141 | for j in range(i, N_LINKS): 142 | J[0, i] -= link_lengths[j] * np.sin(np.sum(joint_angles[:j])) 143 | J[1, i] += link_lengths[j] * np.cos(np.sum(joint_angles[:j])) 144 | 145 | return np.linalg.pinv(J) 146 | 147 | 148 | def distance_to_goal(current_pos, goal_pos): 149 | x_diff = goal_pos[0] - current_pos[0] 150 | y_diff = goal_pos[1] - current_pos[1] 151 | return np.array([x_diff, y_diff]).T, np.math.sqrt(x_diff**2 + y_diff**2) 152 | 153 | 154 | def ang_diff(theta1, theta2): 155 | """ 156 | Returns the difference between two angles in the range -pi to +pi 157 | """ 158 | return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi 159 | 160 | 161 | if __name__ == '__main__': 162 | # main() 163 | animation() -------------------------------------------------------------------------------- /ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Inverse kinematics of a two-joint arm 3 | Left-click the plot to set the goal position of the end effector 4 | 5 | Author: Daniel Ingram (daniel-s-ingram) 6 | Atsushi Sakai (@Atsushi_twi) 7 | 8 | Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 p102 9 | - [Robotics, Vision and Control \| SpringerLink](https://link.springer.com/book/10.1007/978-3-642-20144-8) 10 | 11 | """ 12 | 13 | import matplotlib.pyplot as plt 14 | import numpy as np 15 | 16 | 17 | # Similation parameters 18 | Kp = 15 19 | dt = 0.01 20 | 21 | # Link lengths 22 | l1 = l2 = 1 23 | 24 | # Set initial goal position to the initial end-effector position 25 | x = 2 26 | y = 0 27 | 28 | show_animation = True 29 | 30 | if show_animation: 31 | plt.ion() 32 | 33 | 34 | def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0): 35 | """ 36 | Computes the inverse kinematics for a planar 2DOF arm 37 | """ 38 | while True: 39 | try: 40 | theta2_goal = np.arccos( 41 | (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)) 42 | theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 * 43 | np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) 44 | 45 | if theta1_goal < 0: 46 | theta2_goal = -theta2_goal 47 | theta1_goal = np.math.atan2( 48 | y, x) - np.math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) 49 | 50 | theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt 51 | theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt 52 | except ValueError as e: 53 | print("Unreachable goal") 54 | 55 | wrist = plot_arm(theta1, theta2, x, y) 56 | 57 | # check goal 58 | d2goal = np.math.sqrt((wrist[0] - x)**2 + (wrist[1] - y)**2) 59 | 60 | if abs(d2goal) < GOAL_TH and x is not None: 61 | return theta1, theta2 62 | 63 | 64 | def plot_arm(theta1, theta2, x, y): # pragma: no cover 65 | shoulder = np.array([0, 0]) 66 | elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)]) 67 | wrist = elbow + \ 68 | np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)]) 69 | 70 | if show_animation: 71 | plt.cla() 72 | 73 | plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-') 74 | plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-') 75 | 76 | plt.plot(shoulder[0], shoulder[1], 'ro') 77 | plt.plot(elbow[0], elbow[1], 'ro') 78 | plt.plot(wrist[0], wrist[1], 'ro') 79 | 80 | plt.plot([wrist[0], x], [wrist[1], y], 'g--') 81 | plt.plot(x, y, 'g*') 82 | 83 | plt.xlim(-2, 2) 84 | plt.ylim(-2, 2) 85 | 86 | plt.show() 87 | plt.pause(dt) 88 | 89 | return wrist 90 | 91 | 92 | def ang_diff(theta1, theta2): 93 | # Returns the difference between two angles in the range -pi to +pi 94 | return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi 95 | 96 | 97 | def click(event): # pragma: no cover 98 | global x, y 99 | x = event.xdata 100 | y = event.ydata 101 | 102 | 103 | def animation(): 104 | from random import random 105 | global x, y 106 | theta1 = theta2 = 0.0 107 | for i in range(5): 108 | x = 2.0 * random() - 1.0 109 | y = 2.0 * random() - 1.0 110 | theta1, theta2 = two_joint_arm( 111 | GOAL_TH=0.01, theta1=theta1, theta2=theta2) 112 | 113 | 114 | def main(): # pragma: no cover 115 | fig = plt.figure() 116 | fig.canvas.mpl_connect("button_press_event", click) 117 | two_joint_arm() 118 | 119 | 120 | if __name__ == "__main__": 121 | # animation() 122 | main() 123 | -------------------------------------------------------------------------------- /Bipedal/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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 | -------------------------------------------------------------------------------- /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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/Localization/bayes_filter.png -------------------------------------------------------------------------------- /Localization/extended_kalman_filter/ekf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/Localization/extended_kalman_filter/ekf.png -------------------------------------------------------------------------------- /Localization/extended_kalman_filter/extended_kalman_filter.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Extended kalman filter (EKF) localization sample 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import numpy as np 10 | import math 11 | import matplotlib.pyplot as plt 12 | 13 | # Estimation parameter of EKF 14 | Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance 15 | R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance 16 | 17 | # Simulation parameter 18 | Qsim = np.diag([1.0, np.deg2rad(30.0)])**2 19 | Rsim = np.diag([0.5, 0.5])**2 20 | 21 | DT = 0.1 # time tick [s] 22 | SIM_TIME = 50.0 # simulation time [s] 23 | 24 | show_animation = True 25 | 26 | 27 | def calc_input(): 28 | v = 1.0 # [m/s] 29 | yawrate = 0.1 # [rad/s] 30 | u = np.array([[v, yawrate]]).T 31 | return u 32 | 33 | 34 | def observation(xTrue, xd, u): 35 | 36 | xTrue = motion_model(xTrue, u) 37 | 38 | # add noise to gps x-y 39 | zx = xTrue[0, 0] + np.random.randn() * Rsim[0, 0] 40 | zy = xTrue[1, 0] + np.random.randn() * Rsim[1, 1] 41 | z = np.array([[zx, zy]]).T 42 | 43 | # add noise to input 44 | ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0] 45 | ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1] 46 | ud = np.array([[ud1, ud2]]).T 47 | 48 | xd = motion_model(xd, ud) 49 | 50 | return xTrue, z, xd, ud 51 | 52 | 53 | def motion_model(x, u): 54 | 55 | F = np.array([[1.0, 0, 0, 0], 56 | [0, 1.0, 0, 0], 57 | [0, 0, 1.0, 0], 58 | [0, 0, 0, 0]]) 59 | 60 | B = np.array([[DT * math.cos(x[2, 0]), 0], 61 | [DT * math.sin(x[2, 0]), 0], 62 | [0.0, DT], 63 | [1.0, 0.0]]) 64 | 65 | x = F@x + B@u 66 | 67 | return x 68 | 69 | 70 | def observation_model(x): 71 | # Observation Model 72 | H = np.array([ 73 | [1, 0, 0, 0], 74 | [0, 1, 0, 0] 75 | ]) 76 | 77 | z = H@x 78 | 79 | return z 80 | 81 | 82 | def jacobF(x, u): 83 | """ 84 | Jacobian of Motion Model 85 | 86 | motion model 87 | x_{t+1} = x_t+v*dt*cos(yaw) 88 | y_{t+1} = y_t+v*dt*sin(yaw) 89 | yaw_{t+1} = yaw_t+omega*dt 90 | v_{t+1} = v{t} 91 | so 92 | dx/dyaw = -v*dt*sin(yaw) 93 | dx/dv = dt*cos(yaw) 94 | dy/dyaw = v*dt*cos(yaw) 95 | dy/dv = dt*sin(yaw) 96 | """ 97 | yaw = x[2, 0] 98 | v = u[0, 0] 99 | jF = np.array([ 100 | [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)], 101 | [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)], 102 | [0.0, 0.0, 1.0, 0.0], 103 | [0.0, 0.0, 0.0, 1.0]]) 104 | 105 | return jF 106 | 107 | 108 | def jacobH(x): 109 | # Jacobian of Observation Model 110 | jH = np.array([ 111 | [1, 0, 0, 0], 112 | [0, 1, 0, 0] 113 | ]) 114 | 115 | return jH 116 | 117 | 118 | def ekf_estimation(xEst, PEst, z, u): 119 | 120 | # Predict 121 | xPred = motion_model(xEst, u) 122 | jF = jacobF(xPred, u) 123 | PPred = jF@PEst@jF.T + Q 124 | 125 | # Update 126 | jH = jacobH(xPred) 127 | zPred = observation_model(xPred) 128 | y = z - zPred 129 | S = jH@PPred@jH.T + R 130 | K = PPred@jH.T@np.linalg.inv(S) 131 | xEst = xPred + K@y 132 | PEst = (np.eye(len(xEst)) - K@jH)@PPred 133 | 134 | return xEst, PEst 135 | 136 | 137 | def plot_covariance_ellipse(xEst, PEst): # pragma: no cover 138 | Pxy = PEst[0:2, 0:2] 139 | eigval, eigvec = np.linalg.eig(Pxy) 140 | 141 | if eigval[0] >= eigval[1]: 142 | bigind = 0 143 | smallind = 1 144 | else: 145 | bigind = 1 146 | smallind = 0 147 | 148 | t = np.arange(0, 2 * math.pi + 0.1, 0.1) 149 | a = math.sqrt(eigval[bigind]) 150 | b = math.sqrt(eigval[smallind]) 151 | x = [a * math.cos(it) for it in t] 152 | y = [b * math.sin(it) for it in t] 153 | angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0]) 154 | R = np.array([[math.cos(angle), math.sin(angle)], 155 | [-math.sin(angle), math.cos(angle)]]) 156 | fx = R@(np.array([x, y])) 157 | px = np.array(fx[0, :] + xEst[0, 0]).flatten() 158 | py = np.array(fx[1, :] + xEst[1, 0]).flatten() 159 | plt.plot(px, py, "--r") 160 | 161 | 162 | def main(): 163 | print(__file__ + " start!!") 164 | 165 | time = 0.0 166 | 167 | # State Vector [x y yaw v]' 168 | xEst = np.zeros((4, 1)) 169 | xTrue = np.zeros((4, 1)) 170 | PEst = np.eye(4) 171 | 172 | xDR = np.zeros((4, 1)) # Dead reckoning 173 | 174 | # history 175 | hxEst = xEst 176 | hxTrue = xTrue 177 | hxDR = xTrue 178 | hz = np.zeros((2, 1)) 179 | 180 | while SIM_TIME >= time: 181 | time += DT 182 | u = calc_input() 183 | 184 | xTrue, z, xDR, ud = observation(xTrue, xDR, u) 185 | 186 | xEst, PEst = ekf_estimation(xEst, PEst, z, ud) 187 | 188 | # store data history 189 | hxEst = np.hstack((hxEst, xEst)) 190 | hxDR = np.hstack((hxDR, xDR)) 191 | hxTrue = np.hstack((hxTrue, xTrue)) 192 | hz = np.hstack((hz, z)) 193 | 194 | if show_animation: 195 | plt.cla() 196 | plt.plot(hz[0, :], hz[1, :], ".g") 197 | plt.plot(hxTrue[0, :].flatten(), 198 | hxTrue[1, :].flatten(), "-b") 199 | plt.plot(hxDR[0, :].flatten(), 200 | hxDR[1, :].flatten(), "-k") 201 | plt.plot(hxEst[0, :].flatten(), 202 | hxEst[1, :].flatten(), "-r") 203 | plot_covariance_ellipse(xEst, PEst) 204 | plt.axis("equal") 205 | plt.grid(True) 206 | plt.pause(0.001) 207 | 208 | 209 | if __name__ == '__main__': 210 | main() 211 | -------------------------------------------------------------------------------- /Mapping/circle_fitting/circle_fitting.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Object shape recognition with circle fitting 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import matplotlib.pyplot as plt 10 | import math 11 | import random 12 | import numpy as np 13 | 14 | show_animation = True 15 | 16 | 17 | def circle_fitting(x, y): 18 | """ 19 | Circle Fitting with least squared 20 | input: point x-y positions 21 | output cxe x center position 22 | cye y center position 23 | re radius of circle 24 | error: prediction error 25 | """ 26 | 27 | sumx = sum(x) 28 | sumy = sum(y) 29 | sumx2 = sum([ix ** 2 for ix in x]) 30 | sumy2 = sum([iy ** 2 for iy in y]) 31 | sumxy = sum([ix * iy for (ix, iy) in zip(x, y)]) 32 | 33 | F = np.array([[sumx2, sumxy, sumx], 34 | [sumxy, sumy2, sumy], 35 | [sumx, sumy, len(x)]]) 36 | 37 | G = np.array([[-sum([ix ** 3 + ix * iy ** 2 for (ix, iy) in zip(x, y)])], 38 | [-sum([ix ** 2 * iy + iy ** 3 for (ix, iy) in zip(x, y)])], 39 | [-sum([ix ** 2 + iy ** 2 for (ix, iy) in zip(x, y)])]]) 40 | 41 | T = np.linalg.inv(F).dot(G) 42 | 43 | cxe = float(T[0] / -2) 44 | cye = float(T[1] / -2) 45 | re = math.sqrt(cxe**2 + cye**2 - T[2]) 46 | 47 | error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)]) 48 | 49 | return (cxe, cye, re, error) 50 | 51 | 52 | def get_sample_points(cx, cy, cr, angle_reso): 53 | x, y, angle, r = [], [], [], [] 54 | 55 | # points sampling 56 | for theta in np.arange(0.0, 2.0 * math.pi, angle_reso): 57 | nx = cx + cr * math.cos(theta) 58 | ny = cy + cr * math.sin(theta) 59 | nangle = math.atan2(ny, nx) 60 | nr = math.hypot(nx, ny) * random.uniform(0.95, 1.05) 61 | 62 | x.append(nx) 63 | y.append(ny) 64 | angle.append(nangle) 65 | r.append(nr) 66 | 67 | # ray casting filter 68 | rx, ry = ray_casting_filter(x, y, angle, r, angle_reso) 69 | 70 | return rx, ry 71 | 72 | 73 | def ray_casting_filter(xl, yl, thetal, rangel, angle_reso): 74 | rx, ry = [], [] 75 | rangedb = [float("inf") for _ in range( 76 | int(math.floor((math.pi * 2.0) / angle_reso)) + 1)] 77 | 78 | for i, _ in enumerate(thetal): 79 | angleid = math.floor(thetal[i] / angle_reso) 80 | 81 | if rangedb[angleid] > rangel[i]: 82 | rangedb[angleid] = rangel[i] 83 | 84 | for i, _ in enumerate(rangedb): 85 | t = i * angle_reso 86 | if rangedb[i] != float("inf"): 87 | rx.append(rangedb[i] * math.cos(t)) 88 | ry.append(rangedb[i] * math.sin(t)) 89 | 90 | return rx, ry 91 | 92 | 93 | def plot_circle(x, y, size, color="-b"): # pragma: no cover 94 | deg = list(range(0, 360, 5)) 95 | deg.append(0) 96 | xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] 97 | yl = [y + size * math.sin(np.deg2rad(d)) for d in deg] 98 | plt.plot(xl, yl, color) 99 | 100 | 101 | def main(): 102 | 103 | # simulation parameters 104 | simtime = 15.0 # simulation time 105 | dt = 1.0 # time tick 106 | 107 | cx = -2.0 # initial x position of obstacle 108 | cy = -8.0 # initial y position of obstacle 109 | cr = 1.0 # obstacle radious 110 | theta = np.deg2rad(30.0) # obstacle moving direction 111 | angle_reso = np.deg2rad(3.0) # sensor angle resolution 112 | 113 | time = 0.0 114 | while time <= simtime: 115 | time += dt 116 | 117 | cx += math.cos(theta) 118 | cy += math.cos(theta) 119 | 120 | x, y = get_sample_points(cx, cy, cr, angle_reso) 121 | 122 | ex, ey, er, error = circle_fitting(x, y) 123 | print("Error:", error) 124 | 125 | if show_animation: # pragma: no cover 126 | plt.cla() 127 | plt.axis("equal") 128 | plt.plot(0.0, 0.0, "*r") 129 | plot_circle(cx, cy, cr) 130 | plt.plot(x, y, "xr") 131 | plot_circle(ex, ey, er, "-r") 132 | plt.pause(dt) 133 | 134 | print("Done") 135 | 136 | 137 | if __name__ == '__main__': 138 | main() 139 | -------------------------------------------------------------------------------- /Mapping/gaussian_grid_map/gaussian_grid_map.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | 2D gaussian grid map sample 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import math 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy.stats import norm 13 | 14 | EXTEND_AREA = 10.0 # [m] grid map extention length 15 | 16 | show_animation = True 17 | 18 | 19 | def generate_gaussian_grid_map(ox, oy, xyreso, std): 20 | 21 | minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso) 22 | 23 | gmap = [[0.0 for i in range(yw)] for i in range(xw)] 24 | 25 | for ix in range(xw): 26 | for iy in range(yw): 27 | 28 | x = ix * xyreso + minx 29 | y = iy * xyreso + miny 30 | 31 | # Search minimum distance 32 | mindis = float("inf") 33 | for (iox, ioy) in zip(ox, oy): 34 | d = math.sqrt((iox - x)**2 + (ioy - y)**2) 35 | if mindis >= d: 36 | mindis = d 37 | 38 | pdf = (1.0 - norm.cdf(mindis, 0.0, std)) 39 | gmap[ix][iy] = pdf 40 | 41 | return gmap, minx, maxx, miny, maxy 42 | 43 | 44 | def calc_grid_map_config(ox, oy, xyreso): 45 | minx = round(min(ox) - EXTEND_AREA / 2.0) 46 | miny = round(min(oy) - EXTEND_AREA / 2.0) 47 | maxx = round(max(ox) + EXTEND_AREA / 2.0) 48 | maxy = round(max(oy) + EXTEND_AREA / 2.0) 49 | xw = int(round((maxx - minx) / xyreso)) 50 | yw = int(round((maxy - miny) / xyreso)) 51 | 52 | return minx, miny, maxx, maxy, xw, yw 53 | 54 | 55 | def draw_heatmap(data, minx, maxx, miny, maxy, xyreso): 56 | x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso), 57 | slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)] 58 | plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues) 59 | plt.axis("equal") 60 | 61 | 62 | def main(): 63 | print(__file__ + " start!!") 64 | 65 | xyreso = 0.5 # xy grid resolution 66 | STD = 5.0 # standard diviation for gaussian distribution 67 | 68 | for i in range(5): 69 | ox = (np.random.rand(4) - 0.5) * 10.0 70 | oy = (np.random.rand(4) - 0.5) * 10.0 71 | gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map( 72 | ox, oy, xyreso, STD) 73 | 74 | if show_animation: # pragma: no cover 75 | plt.cla() 76 | draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso) 77 | plt.plot(ox, oy, "xr") 78 | plt.plot(0.0, 0.0, "ob") 79 | plt.pause(1.0) 80 | 81 | 82 | if __name__ == '__main__': 83 | main() 84 | -------------------------------------------------------------------------------- /Mapping/kmeans_clustering/kmeans_clustering.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Object clustering with k-means algorithm 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import numpy as np 10 | import math 11 | import matplotlib.pyplot as plt 12 | import random 13 | 14 | show_animation = True 15 | 16 | 17 | class Clusters: 18 | 19 | def __init__(self, x, y, nlabel): 20 | self.x = x 21 | self.y = y 22 | self.ndata = len(self.x) 23 | self.nlabel = nlabel 24 | self.labels = [random.randint(0, nlabel - 1) 25 | for _ in range(self.ndata)] 26 | self.cx = [0.0 for _ in range(nlabel)] 27 | self.cy = [0.0 for _ in range(nlabel)] 28 | 29 | 30 | def kmeans_clustering(rx, ry, nc): 31 | 32 | clusters = Clusters(rx, ry, nc) 33 | clusters = calc_centroid(clusters) 34 | 35 | MAX_LOOP = 10 36 | DCOST_TH = 0.1 37 | pcost = 100.0 38 | for loop in range(MAX_LOOP): 39 | # print("Loop:", loop) 40 | clusters, cost = update_clusters(clusters) 41 | clusters = calc_centroid(clusters) 42 | 43 | dcost = abs(cost - pcost) 44 | if dcost < DCOST_TH: 45 | break 46 | pcost = cost 47 | 48 | return clusters 49 | 50 | 51 | def calc_centroid(clusters): 52 | 53 | for ic in range(clusters.nlabel): 54 | x, y = calc_labeled_points(ic, clusters) 55 | ndata = len(x) 56 | clusters.cx[ic] = sum(x) / ndata 57 | clusters.cy[ic] = sum(y) / ndata 58 | 59 | return clusters 60 | 61 | 62 | def update_clusters(clusters): 63 | cost = 0.0 64 | 65 | for ip in range(clusters.ndata): 66 | px = clusters.x[ip] 67 | py = clusters.y[ip] 68 | 69 | dx = [icx - px for icx in clusters.cx] 70 | dy = [icy - py for icy in clusters.cy] 71 | 72 | dlist = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)] 73 | mind = min(dlist) 74 | min_id = dlist.index(mind) 75 | clusters.labels[ip] = min_id 76 | cost += min_id 77 | 78 | return clusters, cost 79 | 80 | 81 | def calc_labeled_points(ic, clusters): 82 | 83 | inds = np.array([i for i in range(clusters.ndata) 84 | if clusters.labels[i] == ic]) 85 | tx = np.array(clusters.x) 86 | ty = np.array(clusters.y) 87 | 88 | x = tx[inds] 89 | y = ty[inds] 90 | 91 | return x, y 92 | 93 | 94 | def calc_raw_data(cx, cy, npoints, rand_d): 95 | 96 | rx, ry = [], [] 97 | 98 | for (icx, icy) in zip(cx, cy): 99 | for _ in range(npoints): 100 | rx.append(icx + rand_d * (random.random() - 0.5)) 101 | ry.append(icy + rand_d * (random.random() - 0.5)) 102 | 103 | return rx, ry 104 | 105 | 106 | def update_positions(cx, cy): 107 | 108 | DX1 = 0.4 109 | DY1 = 0.5 110 | 111 | cx[0] += DX1 112 | cy[0] += DY1 113 | 114 | DX2 = -0.3 115 | DY2 = -0.5 116 | 117 | cx[1] += DX2 118 | cy[1] += DY2 119 | 120 | return cx, cy 121 | 122 | 123 | def calc_association(cx, cy, clusters): 124 | 125 | inds = [] 126 | 127 | for ic, _ in enumerate(cx): 128 | tcx = cx[ic] 129 | tcy = cy[ic] 130 | 131 | dx = [icx - tcx for icx in clusters.cx] 132 | dy = [icy - tcy for icy in clusters.cy] 133 | 134 | dlist = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)] 135 | min_id = dlist.index(min(dlist)) 136 | inds.append(min_id) 137 | 138 | return inds 139 | 140 | 141 | def main(): 142 | print(__file__ + " start!!") 143 | 144 | cx = [0.0, 8.0] 145 | cy = [0.0, 8.0] 146 | npoints = 10 147 | rand_d = 3.0 148 | ncluster = 2 149 | sim_time = 15.0 150 | dt = 1.0 151 | time = 0.0 152 | 153 | while time <= sim_time: 154 | print("Time:", time) 155 | time += dt 156 | 157 | # simulate objects 158 | cx, cy = update_positions(cx, cy) 159 | rx, ry = calc_raw_data(cx, cy, npoints, rand_d) 160 | 161 | clusters = kmeans_clustering(rx, ry, ncluster) 162 | 163 | # for animation 164 | if show_animation: # pragma: no cover 165 | plt.cla() 166 | inds = calc_association(cx, cy, clusters) 167 | for ic in inds: 168 | x, y = calc_labeled_points(ic, clusters) 169 | plt.plot(x, y, "x") 170 | plt.plot(cx, cy, "o") 171 | plt.xlim(-2.0, 10.0) 172 | plt.ylim(-2.0, 10.0) 173 | plt.pause(dt) 174 | 175 | print("Done") 176 | 177 | 178 | if __name__ == '__main__': 179 | main() 180 | -------------------------------------------------------------------------------- /Mapping/raycasting_grid_map/raycasting_grid_map.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Ray casting 2D grid map example 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import math 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | 13 | EXTEND_AREA = 10.0 14 | 15 | show_animation = True 16 | 17 | 18 | def calc_grid_map_config(ox, oy, xyreso): 19 | minx = round(min(ox) - EXTEND_AREA / 2.0) 20 | miny = round(min(oy) - EXTEND_AREA / 2.0) 21 | maxx = round(max(ox) + EXTEND_AREA / 2.0) 22 | maxy = round(max(oy) + EXTEND_AREA / 2.0) 23 | xw = int(round((maxx - minx) / xyreso)) 24 | yw = int(round((maxy - miny) / xyreso)) 25 | 26 | return minx, miny, maxx, maxy, xw, yw 27 | 28 | 29 | class precastDB: 30 | 31 | def __init__(self): 32 | self.px = 0.0 33 | self.py = 0.0 34 | self.d = 0.0 35 | self.angle = 0.0 36 | self.ix = 0 37 | self.iy = 0 38 | 39 | def __str__(self): 40 | return str(self.px) + "," + str(self.py) + "," + str(self.d) + "," + str(self.angle) 41 | 42 | 43 | def atan_zero_to_twopi(y, x): 44 | angle = math.atan2(y, x) 45 | if angle < 0.0: 46 | angle += math.pi * 2.0 47 | 48 | return angle 49 | 50 | 51 | def precasting(minx, miny, xw, yw, xyreso, yawreso): 52 | 53 | precast = [[] for i in range(int(round((math.pi * 2.0) / yawreso)) + 1)] 54 | 55 | for ix in range(xw): 56 | for iy in range(yw): 57 | px = ix * xyreso + minx 58 | py = iy * xyreso + miny 59 | 60 | d = math.sqrt(px**2 + py**2) 61 | angle = atan_zero_to_twopi(py, px) 62 | angleid = int(math.floor(angle / yawreso)) 63 | 64 | pc = precastDB() 65 | 66 | pc.px = px 67 | pc.py = py 68 | pc.d = d 69 | pc.ix = ix 70 | pc.iy = iy 71 | pc.angle = angle 72 | 73 | precast[angleid].append(pc) 74 | 75 | return precast 76 | 77 | 78 | def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso): 79 | 80 | minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso) 81 | 82 | pmap = [[0.0 for i in range(yw)] for i in range(xw)] 83 | 84 | precast = precasting(minx, miny, xw, yw, xyreso, yawreso) 85 | 86 | for (x, y) in zip(ox, oy): 87 | 88 | d = math.sqrt(x**2 + y**2) 89 | angle = atan_zero_to_twopi(y, x) 90 | angleid = int(math.floor(angle / yawreso)) 91 | 92 | gridlist = precast[angleid] 93 | 94 | ix = int(round((x - minx) / xyreso)) 95 | iy = int(round((y - miny) / xyreso)) 96 | 97 | for grid in gridlist: 98 | if grid.d > d: 99 | pmap[grid.ix][grid.iy] = 0.5 100 | 101 | pmap[ix][iy] = 1.0 102 | 103 | return pmap, minx, maxx, miny, maxy, xyreso 104 | 105 | 106 | def draw_heatmap(data, minx, maxx, miny, maxy, xyreso): 107 | x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso), 108 | slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)] 109 | plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues) 110 | plt.axis("equal") 111 | 112 | 113 | def main(): 114 | print(__file__ + " start!!") 115 | 116 | xyreso = 0.25 # x-y grid resolution [m] 117 | yawreso = np.deg2rad(10.0) # yaw angle resolution [rad] 118 | 119 | for i in range(5): 120 | ox = (np.random.rand(4) - 0.5) * 10.0 121 | oy = (np.random.rand(4) - 0.5) * 10.0 122 | pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map( 123 | ox, oy, xyreso, yawreso) 124 | 125 | if show_animation: # pragma: no cover 126 | plt.cla() 127 | draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso) 128 | plt.plot(ox, oy, "xr") 129 | plt.plot(0.0, 0.0, "ob") 130 | plt.pause(1.0) 131 | 132 | 133 | if __name__ == '__main__': 134 | main() 135 | -------------------------------------------------------------------------------- /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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/BSplinePath/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/BSplinePath/bspline_path.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path Plannting with B-Spline 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | import scipy.interpolate as si 12 | 13 | # parameter 14 | N = 3 # B Spline order 15 | 16 | 17 | def bspline_planning(x, y, sn): 18 | t = range(len(x)) 19 | x_tup = si.splrep(t, x, k=N) 20 | y_tup = si.splrep(t, y, k=N) 21 | 22 | x_list = list(x_tup) 23 | xl = x.tolist() 24 | x_list[1] = xl + [0.0, 0.0, 0.0, 0.0] 25 | 26 | y_list = list(y_tup) 27 | yl = y.tolist() 28 | y_list[1] = yl + [0.0, 0.0, 0.0, 0.0] 29 | 30 | ipl_t = np.linspace(0.0, len(x) - 1, sn) 31 | rx = si.splev(ipl_t, x_list) 32 | ry = si.splev(ipl_t, y_list) 33 | 34 | return rx, ry 35 | 36 | 37 | def main(): 38 | print(__file__ + " start!!") 39 | # way points 40 | x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) 41 | y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) 42 | sn = 100 # sampling number 43 | 44 | rx, ry = bspline_planning(x, y, sn) 45 | 46 | # show results 47 | plt.plot(x, y, '-og', label="Waypoints") 48 | plt.plot(rx, ry, '-r', label="B-Spline path") 49 | plt.grid(True) 50 | plt.legend() 51 | plt.axis("equal") 52 | plt.show() 53 | 54 | 55 | if __name__ == '__main__': 56 | main() 57 | -------------------------------------------------------------------------------- /PathPlanning/BezierPath/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/BezierPath/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/BezierPath/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/BezierPath/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/ClosedLoopRRTStar/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/ClosedLoopRRTStar/Figure_3.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/ClosedLoopRRTStar/Figure_4.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/ClosedLoopRRTStar/Figure_5.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/CubicSpline/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/CubicSpline/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/CubicSpline/Figure_3.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/cubic_spline_planner.py: -------------------------------------------------------------------------------- 1 | """ 2 | Cubic spline planner 3 | 4 | Author: Atsushi Sakai(@Atsushi_twi) 5 | 6 | """ 7 | import math 8 | import numpy as np 9 | import bisect 10 | 11 | 12 | class Spline: 13 | """ 14 | Cubic Spline class 15 | """ 16 | 17 | def __init__(self, x, y): 18 | self.b, self.c, self.d, self.w = [], [], [], [] 19 | 20 | self.x = x 21 | self.y = y 22 | 23 | self.nx = len(x) # dimension of x 24 | h = np.diff(x) 25 | 26 | # calc coefficient c 27 | self.a = [iy for iy in y] 28 | 29 | # calc coefficient c 30 | A = self.__calc_A(h) 31 | B = self.__calc_B(h) 32 | self.c = np.linalg.solve(A, B) 33 | # print(self.c1) 34 | 35 | # calc spline coefficient b and d 36 | for i in range(self.nx - 1): 37 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 38 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 39 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 40 | self.b.append(tb) 41 | 42 | def calc(self, t): 43 | """ 44 | Calc position 45 | 46 | if t is outside of the input x, return None 47 | 48 | """ 49 | 50 | if t < self.x[0]: 51 | return None 52 | elif t > self.x[-1]: 53 | return None 54 | 55 | i = self.__search_index(t) 56 | dx = t - self.x[i] 57 | result = self.a[i] + self.b[i] * dx + \ 58 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 59 | 60 | return result 61 | 62 | def calcd(self, t): 63 | """ 64 | Calc first derivative 65 | 66 | if t is outside of the input x, return None 67 | """ 68 | 69 | if t < self.x[0]: 70 | return None 71 | elif t > self.x[-1]: 72 | return None 73 | 74 | i = self.__search_index(t) 75 | dx = t - self.x[i] 76 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 77 | return result 78 | 79 | def calcdd(self, t): 80 | """ 81 | Calc second derivative 82 | """ 83 | 84 | if t < self.x[0]: 85 | return None 86 | elif t > self.x[-1]: 87 | return None 88 | 89 | i = self.__search_index(t) 90 | dx = t - self.x[i] 91 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 92 | return result 93 | 94 | def __search_index(self, x): 95 | """ 96 | search data segment index 97 | """ 98 | return bisect.bisect(self.x, x) - 1 99 | 100 | def __calc_A(self, h): 101 | """ 102 | calc matrix A for spline coefficient c 103 | """ 104 | A = np.zeros((self.nx, self.nx)) 105 | A[0, 0] = 1.0 106 | for i in range(self.nx - 1): 107 | if i != (self.nx - 2): 108 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 109 | A[i + 1, i] = h[i] 110 | A[i, i + 1] = h[i] 111 | 112 | A[0, 1] = 0.0 113 | A[self.nx - 1, self.nx - 2] = 0.0 114 | A[self.nx - 1, self.nx - 1] = 1.0 115 | # print(A) 116 | return A 117 | 118 | def __calc_B(self, h): 119 | """ 120 | calc matrix B for spline coefficient c 121 | """ 122 | B = np.zeros(self.nx) 123 | for i in range(self.nx - 2): 124 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 125 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 126 | return B 127 | 128 | 129 | class Spline2D: 130 | """ 131 | 2D Cubic Spline class 132 | 133 | """ 134 | 135 | def __init__(self, x, y): 136 | self.s = self.__calc_s(x, y) 137 | self.sx = Spline(self.s, x) 138 | self.sy = Spline(self.s, y) 139 | 140 | def __calc_s(self, x, y): 141 | dx = np.diff(x) 142 | dy = np.diff(y) 143 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 144 | for (idx, idy) in zip(dx, dy)] 145 | s = [0] 146 | s.extend(np.cumsum(self.ds)) 147 | return s 148 | 149 | def calc_position(self, s): 150 | """ 151 | calc position 152 | """ 153 | x = self.sx.calc(s) 154 | y = self.sy.calc(s) 155 | 156 | return x, y 157 | 158 | def calc_curvature(self, s): 159 | """ 160 | calc curvature 161 | """ 162 | dx = self.sx.calcd(s) 163 | ddx = self.sx.calcdd(s) 164 | dy = self.sy.calcd(s) 165 | ddy = self.sy.calcdd(s) 166 | k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) 167 | return k 168 | 169 | def calc_yaw(self, s): 170 | """ 171 | calc yaw 172 | """ 173 | dx = self.sx.calcd(s) 174 | dy = self.sy.calcd(s) 175 | yaw = math.atan2(dy, dx) 176 | return yaw 177 | 178 | 179 | def calc_spline_course(x, y, ds=0.1): 180 | sp = Spline2D(x, y) 181 | s = list(np.arange(0, sp.s[-1], ds)) 182 | 183 | rx, ry, ryaw, rk = [], [], [], [] 184 | for i_s in s: 185 | ix, iy = sp.calc_position(i_s) 186 | rx.append(ix) 187 | ry.append(iy) 188 | ryaw.append(sp.calc_yaw(i_s)) 189 | rk.append(sp.calc_curvature(i_s)) 190 | 191 | return rx, ry, ryaw, rk, s 192 | 193 | 194 | def main(): 195 | print("Spline 2D test") 196 | import matplotlib.pyplot as plt 197 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 198 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 199 | ds = 0.1 # [m] distance of each intepolated points 200 | 201 | sp = Spline2D(x, y) 202 | s = np.arange(0, sp.s[-1], ds) 203 | 204 | rx, ry, ryaw, rk = [], [], [], [] 205 | for i_s in s: 206 | ix, iy = sp.calc_position(i_s) 207 | rx.append(ix) 208 | ry.append(iy) 209 | ryaw.append(sp.calc_yaw(i_s)) 210 | rk.append(sp.calc_curvature(i_s)) 211 | 212 | plt.subplots(1) 213 | plt.plot(x, y, "xb", label="input") 214 | plt.plot(rx, ry, "-r", label="spline") 215 | plt.grid(True) 216 | plt.axis("equal") 217 | plt.xlabel("x[m]") 218 | plt.ylabel("y[m]") 219 | plt.legend() 220 | 221 | plt.subplots(1) 222 | plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") 223 | plt.grid(True) 224 | plt.legend() 225 | plt.xlabel("line length[m]") 226 | plt.ylabel("yaw angle[deg]") 227 | 228 | plt.subplots(1) 229 | plt.plot(s, rk, "-r", label="curvature") 230 | plt.grid(True) 231 | plt.legend() 232 | plt.xlabel("line length[m]") 233 | plt.ylabel("curvature [1/m]") 234 | 235 | plt.show() 236 | 237 | 238 | if __name__ == '__main__': 239 | main() 240 | -------------------------------------------------------------------------------- /PathPlanning/Dijkstra/dijkstra.py: -------------------------------------------------------------------------------- 1 | """ 2 | Dijkstra grid based planning 3 | 4 | author: Atsushi Sakai(@Atsushi_twi) 5 | """ 6 | 7 | import matplotlib.pyplot as plt 8 | import math 9 | 10 | show_animation = True 11 | 12 | 13 | class Node: 14 | 15 | def __init__(self, x, y, cost, pind): 16 | self.x = x 17 | self.y = y 18 | self.cost = cost 19 | self.pind = pind 20 | 21 | def __str__(self): 22 | return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind) 23 | 24 | 25 | def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr): 26 | """ 27 | gx: goal x position [m] 28 | gx: goal x position [m] 29 | ox: x position list of Obstacles [m] 30 | oy: y position list of Obstacles [m] 31 | reso: grid resolution [m] 32 | rr: robot radius[m] 33 | """ 34 | 35 | nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1) 36 | ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1) 37 | ox = [iox / reso for iox in ox] 38 | oy = [ioy / reso for ioy in oy] 39 | 40 | obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr) 41 | 42 | motion = get_motion_model() 43 | 44 | openset, closedset = dict(), dict() 45 | openset[calc_index(nstart, xw, minx, miny)] = nstart 46 | 47 | while 1: 48 | c_id = min(openset, key=lambda o: openset[o].cost) 49 | current = openset[c_id] 50 | # print("current", current) 51 | 52 | # show graph 53 | if show_animation: 54 | plt.plot(current.x * reso, current.y * reso, "xc") 55 | if len(closedset.keys()) % 10 == 0: 56 | plt.pause(0.001) 57 | 58 | if current.x == ngoal.x and current.y == ngoal.y: 59 | print("Find goal") 60 | ngoal.pind = current.pind 61 | ngoal.cost = current.cost 62 | break 63 | 64 | # Remove the item from the open set 65 | del openset[c_id] 66 | # Add it to the closed set 67 | closedset[c_id] = current 68 | 69 | # expand search grid based on motion model 70 | for i, _ in enumerate(motion): 71 | node = Node(current.x + motion[i][0], current.y + motion[i][1], 72 | current.cost + motion[i][2], c_id) 73 | n_id = calc_index(node, xw, minx, miny) 74 | 75 | if not verify_node(node, obmap, minx, miny, maxx, maxy): 76 | continue 77 | 78 | if n_id in closedset: 79 | continue 80 | # Otherwise if it is already in the open set 81 | if n_id in openset: 82 | if openset[n_id].cost > node.cost: 83 | openset[n_id].cost = node.cost 84 | openset[n_id].pind = c_id 85 | else: 86 | openset[n_id] = node 87 | 88 | rx, ry = calc_final_path(ngoal, closedset, reso) 89 | 90 | return rx, ry 91 | 92 | 93 | def calc_final_path(ngoal, closedset, reso): 94 | # generate final course 95 | rx, ry = [ngoal.x * reso], [ngoal.y * reso] 96 | pind = ngoal.pind 97 | while pind != -1: 98 | n = closedset[pind] 99 | rx.append(n.x * reso) 100 | ry.append(n.y * reso) 101 | pind = n.pind 102 | 103 | return rx, ry 104 | 105 | 106 | def verify_node(node, obmap, minx, miny, maxx, maxy): 107 | 108 | if obmap[node.x][node.y]: 109 | return False 110 | 111 | if node.x < minx: 112 | return False 113 | elif node.y < miny: 114 | return False 115 | elif node.x > maxx: 116 | return False 117 | elif node.y > maxy: 118 | return False 119 | 120 | return True 121 | 122 | 123 | def calc_obstacle_map(ox, oy, reso, vr): 124 | 125 | minx = round(min(ox)) 126 | miny = round(min(oy)) 127 | maxx = round(max(ox)) 128 | maxy = round(max(oy)) 129 | # print("minx:", minx) 130 | # print("miny:", miny) 131 | # print("maxx:", maxx) 132 | # print("maxy:", maxy) 133 | 134 | xwidth = round(maxx - minx) 135 | ywidth = round(maxy - miny) 136 | # print("xwidth:", xwidth) 137 | # print("ywidth:", ywidth) 138 | 139 | # obstacle map generation 140 | obmap = [[False for i in range(xwidth)] for i in range(ywidth)] 141 | for ix in range(xwidth): 142 | x = ix + minx 143 | for iy in range(ywidth): 144 | y = iy + miny 145 | # print(x, y) 146 | for iox, ioy in zip(ox, oy): 147 | d = math.sqrt((iox - x)**2 + (ioy - y)**2) 148 | if d <= vr / reso: 149 | obmap[ix][iy] = True 150 | break 151 | 152 | return obmap, minx, miny, maxx, maxy, xwidth, ywidth 153 | 154 | 155 | def calc_index(node, xwidth, xmin, ymin): 156 | return (node.y - ymin) * xwidth + (node.x - xmin) 157 | 158 | 159 | def get_motion_model(): 160 | # dx, dy, cost 161 | motion = [[1, 0, 1], 162 | [0, 1, 1], 163 | [-1, 0, 1], 164 | [0, -1, 1], 165 | [-1, -1, math.sqrt(2)], 166 | [-1, 1, math.sqrt(2)], 167 | [1, -1, math.sqrt(2)], 168 | [1, 1, math.sqrt(2)]] 169 | 170 | return motion 171 | 172 | 173 | def main(): 174 | print(__file__ + " start!!") 175 | 176 | # start and goal position 177 | sx = 10.0 # [m] 178 | sy = 10.0 # [m] 179 | gx = 50.0 # [m] 180 | gy = 50.0 # [m] 181 | grid_size = 1.0 # [m] 182 | robot_size = 1.0 # [m] 183 | 184 | ox = [] 185 | oy = [] 186 | 187 | for i in range(60): 188 | ox.append(i) 189 | oy.append(0.0) 190 | for i in range(60): 191 | ox.append(60.0) 192 | oy.append(i) 193 | for i in range(61): 194 | ox.append(i) 195 | oy.append(60.0) 196 | for i in range(61): 197 | ox.append(0.0) 198 | oy.append(i) 199 | for i in range(40): 200 | ox.append(20.0) 201 | oy.append(i) 202 | for i in range(40): 203 | ox.append(40.0) 204 | oy.append(60.0 - i) 205 | 206 | if show_animation: 207 | plt.plot(ox, oy, ".k") 208 | plt.plot(sx, sy, "xr") 209 | plt.plot(gx, gy, "xb") 210 | plt.grid(True) 211 | plt.axis("equal") 212 | 213 | rx, ry = dijkstra_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size) 214 | 215 | if show_animation: 216 | plt.plot(rx, ry, "-r") 217 | plt.show() 218 | 219 | 220 | if __name__ == '__main__': 221 | main() 222 | -------------------------------------------------------------------------------- /PathPlanning/DubinsPath/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/DubinsPath/__init__.py -------------------------------------------------------------------------------- /PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py: -------------------------------------------------------------------------------- 1 | """ 2 | cubic spline planner 3 | 4 | Author: Atsushi Sakai 5 | 6 | """ 7 | import math 8 | import numpy as np 9 | import bisect 10 | 11 | 12 | class Spline: 13 | """ 14 | Cubic Spline class 15 | """ 16 | 17 | def __init__(self, x, y): 18 | self.b, self.c, self.d, self.w = [], [], [], [] 19 | 20 | self.x = x 21 | self.y = y 22 | 23 | self.nx = len(x) # dimension of x 24 | h = np.diff(x) 25 | 26 | # calc coefficient c 27 | self.a = [iy for iy in y] 28 | 29 | # calc coefficient c 30 | A = self.__calc_A(h) 31 | B = self.__calc_B(h) 32 | self.c = np.linalg.solve(A, B) 33 | # print(self.c1) 34 | 35 | # calc spline coefficient b and d 36 | for i in range(self.nx - 1): 37 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 38 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 39 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 40 | self.b.append(tb) 41 | 42 | def calc(self, t): 43 | """ 44 | Calc position 45 | 46 | if t is outside of the input x, return None 47 | 48 | """ 49 | 50 | if t < self.x[0]: 51 | return None 52 | elif t > self.x[-1]: 53 | return None 54 | 55 | i = self.__search_index(t) 56 | dx = t - self.x[i] 57 | result = self.a[i] + self.b[i] * dx + \ 58 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 59 | 60 | return result 61 | 62 | def calcd(self, t): 63 | """ 64 | Calc first derivative 65 | 66 | if t is outside of the input x, return None 67 | """ 68 | 69 | if t < self.x[0]: 70 | return None 71 | elif t > self.x[-1]: 72 | return None 73 | 74 | i = self.__search_index(t) 75 | dx = t - self.x[i] 76 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 77 | return result 78 | 79 | def calcdd(self, t): 80 | """ 81 | Calc second derivative 82 | """ 83 | 84 | if t < self.x[0]: 85 | return None 86 | elif t > self.x[-1]: 87 | return None 88 | 89 | i = self.__search_index(t) 90 | dx = t - self.x[i] 91 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 92 | return result 93 | 94 | def __search_index(self, x): 95 | """ 96 | search data segment index 97 | """ 98 | return bisect.bisect(self.x, x) - 1 99 | 100 | def __calc_A(self, h): 101 | """ 102 | calc matrix A for spline coefficient c 103 | """ 104 | A = np.zeros((self.nx, self.nx)) 105 | A[0, 0] = 1.0 106 | for i in range(self.nx - 1): 107 | if i != (self.nx - 2): 108 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 109 | A[i + 1, i] = h[i] 110 | A[i, i + 1] = h[i] 111 | 112 | A[0, 1] = 0.0 113 | A[self.nx - 1, self.nx - 2] = 0.0 114 | A[self.nx - 1, self.nx - 1] = 1.0 115 | # print(A) 116 | return A 117 | 118 | def __calc_B(self, h): 119 | """ 120 | calc matrix B for spline coefficient c 121 | """ 122 | B = np.zeros(self.nx) 123 | for i in range(self.nx - 2): 124 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 125 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 126 | # print(B) 127 | return B 128 | 129 | 130 | class Spline2D: 131 | """ 132 | 2D Cubic Spline class 133 | 134 | """ 135 | 136 | def __init__(self, x, y): 137 | self.s = self.__calc_s(x, y) 138 | self.sx = Spline(self.s, x) 139 | self.sy = Spline(self.s, y) 140 | 141 | def __calc_s(self, x, y): 142 | dx = np.diff(x) 143 | dy = np.diff(y) 144 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 145 | for (idx, idy) in zip(dx, dy)] 146 | s = [0] 147 | s.extend(np.cumsum(self.ds)) 148 | return s 149 | 150 | def calc_position(self, s): 151 | """ 152 | calc position 153 | """ 154 | x = self.sx.calc(s) 155 | y = self.sy.calc(s) 156 | 157 | return x, y 158 | 159 | def calc_curvature(self, s): 160 | """ 161 | calc curvature 162 | """ 163 | dx = self.sx.calcd(s) 164 | ddx = self.sx.calcdd(s) 165 | dy = self.sy.calcd(s) 166 | ddy = self.sy.calcdd(s) 167 | k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) 168 | return k 169 | 170 | def calc_yaw(self, s): 171 | """ 172 | calc yaw 173 | """ 174 | dx = self.sx.calcd(s) 175 | dy = self.sy.calcd(s) 176 | yaw = math.atan2(dy, dx) 177 | return yaw 178 | 179 | 180 | def calc_spline_course(x, y, ds=0.1): 181 | sp = Spline2D(x, y) 182 | s = list(np.arange(0, sp.s[-1], ds)) 183 | 184 | rx, ry, ryaw, rk = [], [], [], [] 185 | for i_s in s: 186 | ix, iy = sp.calc_position(i_s) 187 | rx.append(ix) 188 | ry.append(iy) 189 | ryaw.append(sp.calc_yaw(i_s)) 190 | rk.append(sp.calc_curvature(i_s)) 191 | 192 | return rx, ry, ryaw, rk, s 193 | 194 | 195 | def main(): # pragma: no cover 196 | print("Spline 2D test") 197 | import matplotlib.pyplot as plt 198 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 199 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 200 | 201 | sp = Spline2D(x, y) 202 | s = np.arange(0, sp.s[-1], 0.1) 203 | 204 | rx, ry, ryaw, rk = [], [], [], [] 205 | for i_s in s: 206 | ix, iy = sp.calc_position(i_s) 207 | rx.append(ix) 208 | ry.append(iy) 209 | ryaw.append(sp.calc_yaw(i_s)) 210 | rk.append(sp.calc_curvature(i_s)) 211 | 212 | plt.subplots(1) 213 | plt.plot(x, y, "xb", label="input") 214 | plt.plot(rx, ry, "-r", label="spline") 215 | plt.grid(True) 216 | plt.axis("equal") 217 | plt.xlabel("x[m]") 218 | plt.ylabel("y[m]") 219 | plt.legend() 220 | 221 | plt.subplots(1) 222 | plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") 223 | plt.grid(True) 224 | plt.legend() 225 | plt.xlabel("line length[m]") 226 | plt.ylabel("yaw angle[deg]") 227 | 228 | plt.subplots(1) 229 | plt.plot(s, rk, "-r", label="curvature") 230 | plt.grid(True) 231 | plt.legend() 232 | plt.xlabel("line length[m]") 233 | plt.ylabel("curvature [1/m]") 234 | 235 | plt.show() 236 | 237 | 238 | if __name__ == '__main__': # pragma: no cover 239 | main() 240 | -------------------------------------------------------------------------------- /PathPlanning/LQRPlanner/LQRplanner.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | LQR local path planning 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import matplotlib.pyplot as plt 10 | import numpy as np 11 | import scipy.linalg as la 12 | import math 13 | import random 14 | 15 | show_animation = True 16 | 17 | MAX_TIME = 100.0 # Maximum simulation time 18 | DT = 0.1 # Time tick 19 | 20 | 21 | def LQRplanning(sx, sy, gx, gy): 22 | 23 | rx, ry = [sx], [sy] 24 | 25 | x = np.array([sx - gx, sy - gy]).reshape(2, 1) # State vector 26 | 27 | # Linear system model 28 | A, B = get_system_model() 29 | 30 | found_path = False 31 | 32 | time = 0.0 33 | while time <= MAX_TIME: 34 | time += DT 35 | 36 | u = LQR_control(A, B, x) 37 | 38 | x = A @ x + B @ u 39 | 40 | rx.append(x[0, 0] + gx) 41 | ry.append(x[1, 0] + gy) 42 | 43 | d = math.sqrt((gx - rx[-1])**2 + (gy - ry[-1])**2) 44 | if d <= 0.1: 45 | # print("Goal!!") 46 | found_path = True 47 | break 48 | 49 | # animation 50 | if show_animation: # pragma: no cover 51 | plt.plot(sx, sy, "or") 52 | plt.plot(gx, gy, "ob") 53 | plt.plot(rx, ry, "-r") 54 | plt.axis("equal") 55 | plt.pause(1.0) 56 | 57 | if not found_path: 58 | print("Cannot found path") 59 | return [], [] 60 | 61 | return rx, ry 62 | 63 | 64 | def solve_DARE(A, B, Q, R): 65 | """ 66 | solve a discrete time_Algebraic Riccati equation (DARE) 67 | """ 68 | X = Q 69 | maxiter = 150 70 | eps = 0.01 71 | 72 | for i in range(maxiter): 73 | Xn = A.T * X * A - A.T * X * B * \ 74 | la.inv(R + B.T * X * B) * B.T * X * A + Q 75 | if (abs(Xn - X)).max() < eps: 76 | break 77 | X = Xn 78 | 79 | return Xn 80 | 81 | 82 | def dlqr(A, B, Q, R): 83 | """Solve the discrete time lqr controller. 84 | x[k+1] = A x[k] + B u[k] 85 | cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] 86 | # ref Bertsekas, p.151 87 | """ 88 | 89 | # first, try to solve the ricatti equation 90 | X = solve_DARE(A, B, Q, R) 91 | 92 | # compute the LQR gain 93 | K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A) 94 | 95 | eigVals, eigVecs = la.eig(A - B @ K) 96 | 97 | return K, X, eigVals 98 | 99 | 100 | def get_system_model(): 101 | 102 | A = np.array([[DT, 1.0], 103 | [0.0, DT]]) 104 | B = np.array([0.0, 1.0]).reshape(2, 1) 105 | 106 | return A, B 107 | 108 | 109 | def LQR_control(A, B, x): 110 | 111 | Kopt, X, ev = dlqr(A, B, np.eye(2), np.eye(1)) 112 | 113 | u = -Kopt @ x 114 | 115 | return u 116 | 117 | 118 | def main(): 119 | print(__file__ + " start!!") 120 | 121 | ntest = 10 # number of goal 122 | area = 100.0 # sampling area 123 | 124 | for i in range(ntest): 125 | sx = 6.0 126 | sy = 6.0 127 | gx = random.uniform(-area, area) 128 | gy = random.uniform(-area, area) 129 | 130 | rx, ry = LQRplanning(sx, sy, gx, gy) 131 | 132 | if show_animation: # pragma: no cover 133 | plt.plot(sx, sy, "or") 134 | plt.plot(gx, gy, "ob") 135 | plt.plot(rx, ry, "-r") 136 | plt.axis("equal") 137 | plt.pause(1.0) 138 | 139 | 140 | if __name__ == '__main__': 141 | main() 142 | -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/ModelPredictiveTrajectoryGenerator/__init__.py -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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 numpy as np 10 | import matplotlib.pyplot as plt 11 | import math 12 | import motion_model 13 | 14 | # optimization parameter 15 | max_iter = 100 16 | h = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance 17 | cost_th = 0.1 18 | 19 | show_animation = True 20 | 21 | 22 | def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover 23 | """ 24 | Plot arrow 25 | """ 26 | plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), 27 | fc=fc, ec=ec, head_width=width, head_length=width) 28 | plt.plot(x, y) 29 | plt.plot(0, 0) 30 | 31 | 32 | def calc_diff(target, x, y, yaw): 33 | d = np.array([target.x - x[-1], 34 | target.y - y[-1], 35 | motion_model.pi_2_pi(target.yaw - yaw[-1])]) 36 | 37 | return d 38 | 39 | 40 | def calc_J(target, p, h, k0): 41 | xp, yp, yawp = motion_model.generate_last_state( 42 | p[0, 0] + h[0], p[1, 0], p[2, 0], k0) 43 | dp = calc_diff(target, [xp], [yp], [yawp]) 44 | xn, yn, yawn = motion_model.generate_last_state( 45 | p[0, 0] - h[0], p[1, 0], p[2, 0], k0) 46 | dn = calc_diff(target, [xn], [yn], [yawn]) 47 | d1 = np.array((dp - dn) / (2.0 * h[0])).reshape(3, 1) 48 | 49 | xp, yp, yawp = motion_model.generate_last_state( 50 | p[0, 0], p[1, 0] + h[1], p[2, 0], k0) 51 | dp = calc_diff(target, [xp], [yp], [yawp]) 52 | xn, yn, yawn = motion_model.generate_last_state( 53 | p[0, 0], p[1, 0] - h[1], p[2, 0], k0) 54 | dn = calc_diff(target, [xn], [yn], [yawn]) 55 | d2 = np.array((dp - dn) / (2.0 * h[1])).reshape(3, 1) 56 | 57 | xp, yp, yawp = motion_model.generate_last_state( 58 | p[0, 0], p[1, 0], p[2, 0] + h[2], k0) 59 | dp = calc_diff(target, [xp], [yp], [yawp]) 60 | xn, yn, yawn = motion_model.generate_last_state( 61 | p[0, 0], p[1, 0], p[2, 0] - h[2], k0) 62 | dn = calc_diff(target, [xn], [yn], [yawn]) 63 | d3 = np.array((dp - dn) / (2.0 * h[2])).reshape(3, 1) 64 | 65 | J = np.hstack((d1, d2, d3)) 66 | 67 | return J 68 | 69 | 70 | def selection_learning_param(dp, p, k0, target): 71 | 72 | mincost = float("inf") 73 | mina = 1.0 74 | maxa = 2.0 75 | da = 0.5 76 | 77 | for a in np.arange(mina, maxa, da): 78 | tp = p + a * dp 79 | xc, yc, yawc = motion_model.generate_last_state( 80 | tp[0], tp[1], tp[2], k0) 81 | dc = calc_diff(target, [xc], [yc], [yawc]) 82 | cost = np.linalg.norm(dc) 83 | 84 | if cost <= mincost and a != 0.0: 85 | mina = a 86 | mincost = cost 87 | 88 | # print(mincost, mina) 89 | # input() 90 | 91 | return mina 92 | 93 | 94 | def show_trajectory(target, xc, yc): # pragma: no cover 95 | plt.clf() 96 | plot_arrow(target.x, target.y, target.yaw) 97 | plt.plot(xc, yc, "-r") 98 | plt.axis("equal") 99 | plt.grid(True) 100 | plt.pause(0.1) 101 | 102 | 103 | def optimize_trajectory(target, k0, p): 104 | for i in range(max_iter): 105 | xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0) 106 | dc = np.array(calc_diff(target, xc, yc, yawc)).reshape(3, 1) 107 | 108 | cost = np.linalg.norm(dc) 109 | if cost <= cost_th: 110 | print("path is ok cost is:" + str(cost)) 111 | break 112 | 113 | J = calc_J(target, p, h, k0) 114 | try: 115 | dp = - np.linalg.inv(J) @ dc 116 | except np.linalg.linalg.LinAlgError: 117 | print("cannot calc path LinAlgError") 118 | xc, yc, yawc, p = None, None, None, None 119 | break 120 | alpha = selection_learning_param(dp, p, k0, target) 121 | 122 | p += alpha * np.array(dp) 123 | # print(p.T) 124 | 125 | if show_animation: # pragma: no cover 126 | show_trajectory(target, xc, yc) 127 | else: 128 | xc, yc, yawc, p = None, None, None, None 129 | print("cannot calc path") 130 | 131 | return xc, yc, yawc, p 132 | 133 | 134 | def test_optimize_trajectory(): # pragma: no cover 135 | 136 | # target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0)) 137 | target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0)) 138 | k0 = 0.0 139 | 140 | init_p = np.array([6.0, 0.0, 0.0]).reshape(3, 1) 141 | 142 | x, y, yaw, p = optimize_trajectory(target, k0, init_p) 143 | 144 | if show_animation: 145 | show_trajectory(target, x, y) 146 | plot_arrow(target.x, target.y, target.yaw) 147 | plt.axis("equal") 148 | plt.grid(True) 149 | plt.show() 150 | 151 | 152 | def main(): # pragma: no cover 153 | print(__file__ + " start!!") 154 | test_optimize_trajectory() 155 | 156 | 157 | if __name__ == '__main__': 158 | main() 159 | -------------------------------------------------------------------------------- /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 | tk = np.array([0.0, time / 2.0, time]) 40 | kk = np.array([k0, km, kf]) 41 | t = np.arange(0.0, time, time / n) 42 | fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") 43 | kp = [fkp(ti) for ti in t] 44 | dt = float(time / n) 45 | 46 | # plt.plot(t, kp) 47 | # plt.show() 48 | 49 | state = State() 50 | x, y, yaw = [state.x], [state.y], [state.yaw] 51 | 52 | for ikp in kp: 53 | state = update(state, v, ikp, dt, L) 54 | x.append(state.x) 55 | y.append(state.y) 56 | yaw.append(state.yaw) 57 | 58 | return x, y, yaw 59 | 60 | 61 | def generate_last_state(s, km, kf, k0): 62 | 63 | n = s / ds 64 | time = s / v # [s] 65 | tk = np.array([0.0, time / 2.0, time]) 66 | kk = np.array([k0, km, kf]) 67 | t = np.arange(0.0, time, time / n) 68 | fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") 69 | kp = [fkp(ti) for ti in t] 70 | dt = time / n 71 | 72 | # plt.plot(t, kp) 73 | # plt.show() 74 | 75 | state = State() 76 | 77 | _ = [update(state, v, ikp, dt, L) for ikp in kp] 78 | 79 | return state.x, state.y, state.yaw 80 | -------------------------------------------------------------------------------- /PathPlanning/PotentialFieldPlanning/potential_field_planning.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Potential Field based path planner 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | Ref: 8 | https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf 9 | 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | 15 | # Parameters 16 | KP = 5.0 # attractive potential gain 17 | ETA = 100.0 # repulsive potential gain 18 | AREA_WIDTH = 30.0 # potential area width [m] 19 | 20 | show_animation = True 21 | 22 | 23 | def calc_potential_field(gx, gy, ox, oy, reso, rr): 24 | minx = min(ox) - AREA_WIDTH / 2.0 25 | miny = min(oy) - AREA_WIDTH / 2.0 26 | maxx = max(ox) + AREA_WIDTH / 2.0 27 | maxy = max(oy) + AREA_WIDTH / 2.0 28 | xw = int(round((maxx - minx) / reso)) 29 | yw = int(round((maxy - miny) / reso)) 30 | 31 | # calc each potential 32 | pmap = [[0.0 for i in range(yw)] for i in range(xw)] 33 | 34 | for ix in range(xw): 35 | x = ix * reso + minx 36 | 37 | for iy in range(yw): 38 | y = iy * reso + miny 39 | ug = calc_attractive_potential(x, y, gx, gy) 40 | uo = calc_repulsive_potential(x, y, ox, oy, rr) 41 | uf = ug + uo 42 | pmap[ix][iy] = uf 43 | 44 | return pmap, minx, miny 45 | 46 | 47 | def calc_attractive_potential(x, y, gx, gy): 48 | return 0.5 * KP * np.hypot(x - gx, y - gy) 49 | 50 | 51 | def calc_repulsive_potential(x, y, ox, oy, rr): 52 | # search nearest obstacle 53 | minid = -1 54 | dmin = float("inf") 55 | for i, _ in enumerate(ox): 56 | d = np.hypot(x - ox[i], y - oy[i]) 57 | if dmin >= d: 58 | dmin = d 59 | minid = i 60 | 61 | # calc repulsive potential 62 | dq = np.hypot(x - ox[minid], y - oy[minid]) 63 | 64 | if dq <= rr: 65 | if dq <= 0.1: 66 | dq = 0.1 67 | 68 | return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2 69 | else: 70 | return 0.0 71 | 72 | 73 | def get_motion_model(): 74 | # dx, dy 75 | motion = [[1, 0], 76 | [0, 1], 77 | [-1, 0], 78 | [0, -1], 79 | [-1, -1], 80 | [-1, 1], 81 | [1, -1], 82 | [1, 1]] 83 | 84 | return motion 85 | 86 | 87 | def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr): 88 | 89 | # calc potential field 90 | pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr) 91 | 92 | # search path 93 | d = np.hypot(sx - gx, sy - gy) 94 | ix = round((sx - minx) / reso) 95 | iy = round((sy - miny) / reso) 96 | gix = round((gx - minx) / reso) 97 | giy = round((gy - miny) / reso) 98 | 99 | if show_animation: 100 | draw_heatmap(pmap) 101 | plt.plot(ix, iy, "*k") 102 | plt.plot(gix, giy, "*m") 103 | 104 | rx, ry = [sx], [sy] 105 | motion = get_motion_model() 106 | while d >= reso: 107 | minp = float("inf") 108 | minix, miniy = -1, -1 109 | for i, _ in enumerate(motion): 110 | inx = int(ix + motion[i][0]) 111 | iny = int(iy + motion[i][1]) 112 | if inx >= len(pmap) or iny >= len(pmap[0]): 113 | p = float("inf") # outside area 114 | else: 115 | p = pmap[inx][iny] 116 | if minp > p: 117 | minp = p 118 | minix = inx 119 | miniy = iny 120 | ix = minix 121 | iy = miniy 122 | xp = ix * reso + minx 123 | yp = iy * reso + miny 124 | d = np.hypot(gx - xp, gy - yp) 125 | rx.append(xp) 126 | ry.append(yp) 127 | 128 | if show_animation: 129 | plt.plot(ix, iy, ".r") 130 | plt.pause(0.01) 131 | 132 | print("Goal!!") 133 | 134 | return rx, ry 135 | 136 | 137 | def draw_heatmap(data): 138 | data = np.array(data).T 139 | plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues) 140 | 141 | 142 | def main(): 143 | print("potential_field_planning start") 144 | 145 | sx = 0.0 # start x position [m] 146 | sy = 10.0 # start y positon [m] 147 | gx = 30.0 # goal x position [m] 148 | gy = 30.0 # goal y position [m] 149 | grid_size = 0.5 # potential grid size [m] 150 | robot_radius = 5.0 # robot radius [m] 151 | 152 | ox = [15.0, 5.0, 20.0, 25.0] # obstacle x position list [m] 153 | oy = [25.0, 15.0, 26.0, 25.0] # obstacle y position list [m] 154 | 155 | if show_animation: 156 | plt.grid(True) 157 | plt.axis("equal") 158 | 159 | # path generation 160 | _, _ = potential_field_planning( 161 | sx, sy, gx, gy, ox, oy, grid_size, robot_radius) 162 | 163 | if show_animation: 164 | plt.show() 165 | 166 | 167 | if __name__ == '__main__': 168 | print(__file__ + " start!!") 169 | main() 170 | print(__file__ + " Done!!") 171 | -------------------------------------------------------------------------------- /PathPlanning/RRT/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/RRT/figure_1.png -------------------------------------------------------------------------------- /PathPlanning/RRT/simple_rrt.py: -------------------------------------------------------------------------------- 1 | """ 2 | Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) 3 | 4 | author: AtsushiSakai(@Atsushi_twi) 5 | 6 | """ 7 | 8 | import matplotlib.pyplot as plt 9 | import random 10 | import math 11 | import copy 12 | 13 | show_animation = True 14 | 15 | 16 | class RRT(): 17 | """ 18 | Class for RRT Planning 19 | """ 20 | 21 | def __init__(self, start, goal, obstacleList, 22 | randArea, expandDis=1.0, goalSampleRate=5, maxIter=500): 23 | """ 24 | Setting Parameter 25 | 26 | start:Start Position [x,y] 27 | goal:Goal Position [x,y] 28 | obstacleList:obstacle Positions [[x,y,size],...] 29 | randArea:Ramdom Samping Area [min,max] 30 | 31 | """ 32 | self.start = Node(start[0], start[1]) 33 | self.end = Node(goal[0], goal[1]) 34 | self.minrand = randArea[0] 35 | self.maxrand = randArea[1] 36 | self.expandDis = expandDis 37 | self.goalSampleRate = goalSampleRate 38 | self.maxIter = maxIter 39 | self.obstacleList = obstacleList 40 | 41 | def Planning(self, animation=True): 42 | """ 43 | Pathplanning 44 | 45 | animation: flag for animation on or off 46 | """ 47 | 48 | self.nodeList = [self.start] 49 | while True: 50 | # Random Sampling 51 | if random.randint(0, 100) > self.goalSampleRate: 52 | rnd = [random.uniform(self.minrand, self.maxrand), random.uniform( 53 | self.minrand, self.maxrand)] 54 | else: 55 | rnd = [self.end.x, self.end.y] 56 | 57 | # Find nearest node 58 | nind = self.GetNearestListIndex(self.nodeList, rnd) 59 | # print(nind) 60 | 61 | # expand tree 62 | nearestNode = self.nodeList[nind] 63 | theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) 64 | 65 | newNode = copy.deepcopy(nearestNode) 66 | newNode.x += self.expandDis * math.cos(theta) 67 | newNode.y += self.expandDis * math.sin(theta) 68 | newNode.parent = nind 69 | 70 | if not self.__CollisionCheck(newNode, self.obstacleList): 71 | continue 72 | 73 | self.nodeList.append(newNode) 74 | print("nNodelist:", len(self.nodeList)) 75 | 76 | # check goal 77 | dx = newNode.x - self.end.x 78 | dy = newNode.y - self.end.y 79 | d = math.sqrt(dx * dx + dy * dy) 80 | if d <= self.expandDis: 81 | print("Goal!!") 82 | break 83 | 84 | if animation: 85 | self.DrawGraph(rnd) 86 | 87 | path = [[self.end.x, self.end.y]] 88 | lastIndex = len(self.nodeList) - 1 89 | while self.nodeList[lastIndex].parent is not None: 90 | node = self.nodeList[lastIndex] 91 | path.append([node.x, node.y]) 92 | lastIndex = node.parent 93 | path.append([self.start.x, self.start.y]) 94 | 95 | return path 96 | 97 | def DrawGraph(self, rnd=None): # pragma: no cover 98 | """ 99 | Draw Graph 100 | """ 101 | plt.clf() 102 | if rnd is not None: 103 | plt.plot(rnd[0], rnd[1], "^k") 104 | for node in self.nodeList: 105 | if node.parent is not None: 106 | plt.plot([node.x, self.nodeList[node.parent].x], [ 107 | node.y, self.nodeList[node.parent].y], "-g") 108 | 109 | for (ox, oy, size) in self.obstacleList: 110 | plt.plot(ox, oy, "ok", ms=30 * size) 111 | 112 | plt.plot(self.start.x, self.start.y, "xr") 113 | plt.plot(self.end.x, self.end.y, "xr") 114 | plt.axis([-2, 15, -2, 15]) 115 | plt.grid(True) 116 | plt.pause(0.01) 117 | 118 | def GetNearestListIndex(self, nodeList, rnd): 119 | dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) 120 | ** 2 for node in nodeList] 121 | minind = dlist.index(min(dlist)) 122 | return minind 123 | 124 | def __CollisionCheck(self, node, obstacleList): 125 | 126 | for (ox, oy, size) in obstacleList: 127 | dx = ox - node.x 128 | dy = oy - node.y 129 | d = math.sqrt(dx * dx + dy * dy) 130 | if d <= size: 131 | return False # collision 132 | 133 | return True # safe 134 | 135 | 136 | class Node(): 137 | """ 138 | RRT Node 139 | """ 140 | 141 | def __init__(self, x, y): 142 | self.x = x 143 | self.y = y 144 | self.parent = None 145 | 146 | 147 | def main(gx=5.0, gy=10.0): 148 | print("start " + __file__) 149 | 150 | # ====Search Path with RRT==== 151 | obstacleList = [ 152 | (5, 5, 1), 153 | (3, 6, 2), 154 | (3, 8, 2), 155 | (3, 10, 2), 156 | (7, 5, 2), 157 | (9, 5, 2) 158 | ] # [x,y,size] 159 | # Set Initial parameters 160 | rrt = RRT(start=[0, 0], goal=[gx, gy], 161 | randArea=[-2, 15], obstacleList=obstacleList) 162 | path = rrt.Planning(animation=show_animation) 163 | 164 | # Draw final path 165 | if show_animation: # pragma: no cover 166 | rrt.DrawGraph() 167 | plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') 168 | plt.grid(True) 169 | plt.show() 170 | 171 | 172 | if __name__ == '__main__': 173 | main() 174 | -------------------------------------------------------------------------------- /PathPlanning/RRTDubins/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/RRTDubins/__init__.py -------------------------------------------------------------------------------- /PathPlanning/RRTStarReedsShepp/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/RRTStarReedsShepp/figure_1.png -------------------------------------------------------------------------------- /PathPlanning/RRTstar/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/RRTstar/figure_1.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/StateLatticePlanner/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/StateLatticePlanner/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/StateLatticePlanner/Figure_3.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/StateLatticePlanner/Figure_4.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/StateLatticePlanner/Figure_5.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/StateLatticePlanner/Figure_6.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathPlanning/__init__.py -------------------------------------------------------------------------------- /PathTracking/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathTracking/.gitignore -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathTracking/cgmres_nmpc/Figure_1.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathTracking/cgmres_nmpc/Figure_2.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathTracking/cgmres_nmpc/Figure_3.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathTracking/cgmres_nmpc/Figure_4.png -------------------------------------------------------------------------------- /PathTracking/lqr_steer_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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.sqrt(x_diff**2 + y_diff**2) 44 | while rho > 0.001: 45 | x_traj.append(x) 46 | y_traj.append(y) 47 | 48 | x_diff = x_goal - x 49 | y_diff = y_goal - y 50 | 51 | # Restrict alpha and beta (angle differences) to the range 52 | # [-pi, pi] to prevent unstable behavior e.g. difference going 53 | # from 0 rad to 2*pi rad with slight turn 54 | 55 | rho = np.sqrt(x_diff**2 + y_diff**2) 56 | alpha = (np.arctan2(y_diff, x_diff) 57 | - theta + np.pi) % (2 * np.pi) - np.pi 58 | beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi 59 | 60 | v = Kp_rho * rho 61 | w = Kp_alpha * alpha + Kp_beta * beta 62 | 63 | if alpha > np.pi / 2 or alpha < -np.pi / 2: 64 | v = -v 65 | 66 | theta = theta + w * dt 67 | x = x + v * np.cos(theta) * dt 68 | y = y + v * np.sin(theta) * dt 69 | 70 | if show_animation: # pragma: no cover 71 | plt.cla() 72 | plt.arrow(x_start, y_start, np.cos(theta_start), 73 | np.sin(theta_start), color='r', width=0.1) 74 | plt.arrow(x_goal, y_goal, np.cos(theta_goal), 75 | np.sin(theta_goal), color='g', width=0.1) 76 | plot_vehicle(x, y, theta, x_traj, y_traj) 77 | 78 | 79 | def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover 80 | # Corners of triangular vehicle when pointing to the right (0 radians) 81 | p1_i = np.array([0.5, 0, 1]).T 82 | p2_i = np.array([-0.5, 0.25, 1]).T 83 | p3_i = np.array([-0.5, -0.25, 1]).T 84 | 85 | T = transformation_matrix(x, y, theta) 86 | p1 = np.matmul(T, p1_i) 87 | p2 = np.matmul(T, p2_i) 88 | p3 = np.matmul(T, p3_i) 89 | 90 | plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-') 91 | plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-') 92 | plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-') 93 | 94 | plt.plot(x_traj, y_traj, 'b--') 95 | 96 | plt.xlim(0, 20) 97 | plt.ylim(0, 20) 98 | 99 | plt.pause(dt) 100 | 101 | 102 | def transformation_matrix(x, y, theta): 103 | return np.array([ 104 | [np.cos(theta), -np.sin(theta), x], 105 | [np.sin(theta), np.cos(theta), y], 106 | [0, 0, 1] 107 | ]) 108 | 109 | 110 | def main(): 111 | 112 | for i in range(5): 113 | x_start = 20 * random() 114 | y_start = 20 * random() 115 | theta_start = 2 * np.pi * random() - np.pi 116 | x_goal = 20 * random() 117 | y_goal = 20 * random() 118 | theta_goal = 2 * np.pi * random() - np.pi 119 | print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % 120 | (x_start, y_start, theta_start)) 121 | print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % 122 | (x_goal, y_goal, theta_goal)) 123 | move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal) 124 | 125 | 126 | if __name__ == '__main__': 127 | main() 128 | -------------------------------------------------------------------------------- /PathTracking/pure_pursuit/pure_pursuit.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path tracking simulation with pure pursuit steering control and PID speed control. 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | import numpy as np 9 | import math 10 | import matplotlib.pyplot as plt 11 | 12 | k = 0.1 # look forward gain 13 | Lfc = 1.0 # look-ahead distance 14 | Kp = 1.0 # speed propotional gain 15 | dt = 0.1 # [s] 16 | L = 2.9 # [m] wheel base of vehicle 17 | 18 | 19 | show_animation = True 20 | 21 | 22 | class State: 23 | 24 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 25 | self.x = x 26 | self.y = y 27 | self.yaw = yaw 28 | self.v = v 29 | 30 | 31 | def update(state, a, delta): 32 | 33 | state.x = state.x + state.v * math.cos(state.yaw) * dt 34 | state.y = state.y + state.v * math.sin(state.yaw) * dt 35 | state.yaw = state.yaw + state.v / L * math.tan(delta) * dt 36 | state.v = state.v + a * dt 37 | 38 | return state 39 | 40 | 41 | def PIDControl(target, current): 42 | a = Kp * (target - current) 43 | 44 | return a 45 | 46 | 47 | def pure_pursuit_control(state, cx, cy, pind): 48 | 49 | ind = calc_target_index(state, cx, cy) 50 | 51 | if pind >= ind: 52 | ind = pind 53 | 54 | if ind < len(cx): 55 | tx = cx[ind] 56 | ty = cy[ind] 57 | else: 58 | tx = cx[-1] 59 | ty = cy[-1] 60 | ind = len(cx) - 1 61 | 62 | alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw 63 | 64 | if state.v < 0: # back 65 | alpha = math.pi - alpha 66 | 67 | Lf = k * state.v + Lfc 68 | 69 | delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) 70 | 71 | return delta, ind 72 | 73 | 74 | def calc_target_index(state, cx, cy): 75 | 76 | # search nearest point index 77 | dx = [state.x - icx for icx in cx] 78 | dy = [state.y - icy for icy in cy] 79 | d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] 80 | ind = d.index(min(d)) 81 | L = 0.0 82 | 83 | Lf = k * state.v + Lfc 84 | 85 | # search look ahead target point index 86 | while Lf > L and (ind + 1) < len(cx): 87 | dx = cx[ind + 1] - cx[ind] 88 | dy = cy[ind + 1] - cy[ind] 89 | L += math.sqrt(dx ** 2 + dy ** 2) 90 | ind += 1 91 | 92 | return ind 93 | 94 | 95 | def main(): 96 | # target course 97 | cx = np.arange(0, 50, 0.1) 98 | cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx] 99 | 100 | target_speed = 10.0 / 3.6 # [m/s] 101 | 102 | T = 100.0 # max simulation time 103 | 104 | # initial state 105 | state = State(x=-0.0, y=-3.0, yaw=0.0, v=0.0) 106 | 107 | lastIndex = len(cx) - 1 108 | time = 0.0 109 | x = [state.x] 110 | y = [state.y] 111 | yaw = [state.yaw] 112 | v = [state.v] 113 | t = [0.0] 114 | target_ind = calc_target_index(state, cx, cy) 115 | 116 | while T >= time and lastIndex > target_ind: 117 | ai = PIDControl(target_speed, state.v) 118 | di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) 119 | state = update(state, ai, di) 120 | 121 | time = time + dt 122 | 123 | x.append(state.x) 124 | y.append(state.y) 125 | yaw.append(state.yaw) 126 | v.append(state.v) 127 | t.append(time) 128 | 129 | if show_animation: # pragma: no cover 130 | plt.cla() 131 | plt.plot(cx, cy, ".r", label="course") 132 | plt.plot(x, y, "-b", label="trajectory") 133 | plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") 134 | plt.axis("equal") 135 | plt.grid(True) 136 | plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4]) 137 | plt.pause(0.001) 138 | 139 | # Test 140 | assert lastIndex >= target_ind, "Cannot goal" 141 | 142 | if show_animation: # pragma: no cover 143 | plt.cla() 144 | plt.plot(cx, cy, ".r", label="course") 145 | plt.plot(x, y, "-b", label="trajectory") 146 | plt.legend() 147 | plt.xlabel("x[m]") 148 | plt.ylabel("y[m]") 149 | plt.axis("equal") 150 | plt.grid(True) 151 | 152 | plt.subplots(1) 153 | plt.plot(t, [iv * 3.6 for iv in v], "-r") 154 | plt.xlabel("Time[s]") 155 | plt.ylabel("Speed[km/h]") 156 | plt.grid(True) 157 | plt.show() 158 | 159 | 160 | if __name__ == '__main__': 161 | print("Pure pursuit path tracking simulation start") 162 | main() 163 | -------------------------------------------------------------------------------- /PathTracking/rear_wheel_feedback/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/PathTracking/rear_wheel_feedback/Figure_2.png -------------------------------------------------------------------------------- /PathTracking/rear_wheel_feedback/rear_wheel_feedback.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path tracking simulation with rear wheel feedback steering control and PID speed control. 4 | 5 | author: Atsushi Sakai(@Atsushi_twi) 6 | 7 | """ 8 | import matplotlib.pyplot as plt 9 | import math 10 | import numpy as np 11 | import sys 12 | sys.path.append("../../PathPlanning/CubicSpline/") 13 | 14 | try: 15 | import cubic_spline_planner 16 | except: 17 | raise 18 | 19 | 20 | Kp = 1.0 # speed propotional gain 21 | # steering control parameter 22 | KTH = 1.0 23 | KE = 0.5 24 | 25 | dt = 0.1 # [s] 26 | L = 2.9 # [m] 27 | 28 | show_animation = True 29 | # show_animation = False 30 | 31 | 32 | class State: 33 | 34 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 35 | self.x = x 36 | self.y = y 37 | self.yaw = yaw 38 | self.v = v 39 | 40 | 41 | def update(state, a, delta): 42 | 43 | state.x = state.x + state.v * math.cos(state.yaw) * dt 44 | state.y = state.y + state.v * math.sin(state.yaw) * dt 45 | state.yaw = state.yaw + state.v / L * math.tan(delta) * dt 46 | state.v = state.v + a * dt 47 | 48 | return state 49 | 50 | 51 | def PIDControl(target, current): 52 | a = Kp * (target - current) 53 | 54 | return a 55 | 56 | 57 | def pi_2_pi(angle): 58 | while(angle > math.pi): 59 | angle = angle - 2.0 * math.pi 60 | 61 | while(angle < -math.pi): 62 | angle = angle + 2.0 * math.pi 63 | 64 | return angle 65 | 66 | 67 | def rear_wheel_feedback_control(state, cx, cy, cyaw, ck, preind): 68 | ind, e = calc_nearest_index(state, cx, cy, cyaw) 69 | 70 | k = ck[ind] 71 | v = state.v 72 | th_e = pi_2_pi(state.yaw - cyaw[ind]) 73 | 74 | omega = v * k * math.cos(th_e) / (1.0 - k * e) - \ 75 | KTH * abs(v) * th_e - KE * v * math.sin(th_e) * e / th_e 76 | 77 | if th_e == 0.0 or omega == 0.0: 78 | return 0.0, ind 79 | 80 | delta = math.atan2(L * omega / v, 1.0) 81 | # print(k, v, e, th_e, omega, delta) 82 | 83 | return delta, ind 84 | 85 | 86 | def calc_nearest_index(state, cx, cy, cyaw): 87 | dx = [state.x - icx for icx in cx] 88 | dy = [state.y - icy for icy in cy] 89 | 90 | d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] 91 | 92 | mind = min(d) 93 | 94 | ind = d.index(mind) 95 | 96 | mind = math.sqrt(mind) 97 | 98 | dxl = cx[ind] - state.x 99 | dyl = cy[ind] - state.y 100 | 101 | angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl)) 102 | if angle < 0: 103 | mind *= -1 104 | 105 | return ind, mind 106 | 107 | 108 | def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): 109 | 110 | T = 500.0 # max simulation time 111 | goal_dis = 0.3 112 | stop_speed = 0.05 113 | 114 | state = State(x=-0.0, y=-0.0, yaw=0.0, v=0.0) 115 | 116 | time = 0.0 117 | x = [state.x] 118 | y = [state.y] 119 | yaw = [state.yaw] 120 | v = [state.v] 121 | t = [0.0] 122 | goal_flag = False 123 | target_ind = calc_nearest_index(state, cx, cy, cyaw) 124 | 125 | while T >= time: 126 | di, target_ind = rear_wheel_feedback_control( 127 | state, cx, cy, cyaw, ck, target_ind) 128 | ai = PIDControl(speed_profile[target_ind], state.v) 129 | state = update(state, ai, di) 130 | 131 | if abs(state.v) <= stop_speed: 132 | target_ind += 1 133 | 134 | time = time + dt 135 | 136 | # check goal 137 | dx = state.x - goal[0] 138 | dy = state.y - goal[1] 139 | if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis: 140 | print("Goal") 141 | goal_flag = True 142 | break 143 | 144 | x.append(state.x) 145 | y.append(state.y) 146 | yaw.append(state.yaw) 147 | v.append(state.v) 148 | t.append(time) 149 | 150 | if target_ind % 1 == 0 and show_animation: 151 | plt.cla() 152 | plt.plot(cx, cy, "-r", label="course") 153 | plt.plot(x, y, "ob", label="trajectory") 154 | plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") 155 | plt.axis("equal") 156 | plt.grid(True) 157 | plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) + 158 | ",target index:" + str(target_ind)) 159 | plt.pause(0.0001) 160 | 161 | return t, x, y, yaw, v, goal_flag 162 | 163 | 164 | def calc_speed_profile(cx, cy, cyaw, target_speed): 165 | 166 | speed_profile = [target_speed] * len(cx) 167 | 168 | direction = 1.0 169 | 170 | # Set stop point 171 | for i in range(len(cx) - 1): 172 | dyaw = cyaw[i + 1] - cyaw[i] 173 | switch = math.pi / 4.0 <= dyaw < math.pi / 2.0 174 | 175 | if switch: 176 | direction *= -1 177 | 178 | if direction != 1.0: 179 | speed_profile[i] = - target_speed 180 | else: 181 | speed_profile[i] = target_speed 182 | 183 | if switch: 184 | speed_profile[i] = 0.0 185 | 186 | speed_profile[-1] = 0.0 187 | 188 | return speed_profile 189 | 190 | 191 | def main(): 192 | print("rear wheel feedback tracking start!!") 193 | ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0] 194 | ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0] 195 | goal = [ax[-1], ay[-1]] 196 | 197 | cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course( 198 | ax, ay, ds=0.1) 199 | target_speed = 10.0 / 3.6 200 | 201 | sp = calc_speed_profile(cx, cy, cyaw, target_speed) 202 | 203 | t, x, y, yaw, v, goal_flag = closed_loop_prediction( 204 | cx, cy, cyaw, ck, sp, goal) 205 | 206 | # Test 207 | assert goal_flag, "Cannot goal" 208 | 209 | if show_animation: # pragma: no cover 210 | plt.close() 211 | plt.subplots(1) 212 | plt.plot(ax, ay, "xb", label="input") 213 | plt.plot(cx, cy, "-r", label="spline") 214 | plt.plot(x, y, "-g", label="tracking") 215 | plt.grid(True) 216 | plt.axis("equal") 217 | plt.xlabel("x[m]") 218 | plt.ylabel("y[m]") 219 | plt.legend() 220 | 221 | plt.subplots(1) 222 | plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") 223 | plt.grid(True) 224 | plt.legend() 225 | plt.xlabel("line length[m]") 226 | plt.ylabel("yaw angle[deg]") 227 | 228 | plt.subplots(1) 229 | plt.plot(s, ck, "-r", label="curvature") 230 | plt.grid(True) 231 | plt.legend() 232 | plt.xlabel("line length[m]") 233 | plt.ylabel("curvature [1/m]") 234 | 235 | plt.show() 236 | 237 | 238 | if __name__ == '__main__': 239 | main() 240 | -------------------------------------------------------------------------------- /SLAM/FastSLAM1/animation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/SLAM/FastSLAM1/animation.png -------------------------------------------------------------------------------- /SLAM/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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) 4 | """ 5 | 6 | import math 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | # ICP parameters 11 | EPS = 0.0001 12 | MAXITER = 100 13 | 14 | show_animation = True 15 | 16 | 17 | def ICP_matching(ppoints, cpoints): 18 | """ 19 | Iterative Closest Point matching 20 | - input 21 | ppoints: 2D points in the previous frame 22 | cpoints: 2D points in the current frame 23 | - output 24 | R: Rotation matrix 25 | T: Translation vector 26 | """ 27 | H = None # homogeneous transformation matrix 28 | 29 | dError = 1000.0 30 | preError = 1000.0 31 | count = 0 32 | 33 | while dError >= EPS: 34 | count += 1 35 | 36 | if show_animation: # pragma: no cover 37 | plt.cla() 38 | plt.plot(ppoints[0, :], ppoints[1, :], ".r") 39 | plt.plot(cpoints[0, :], cpoints[1, :], ".b") 40 | plt.plot(0.0, 0.0, "xr") 41 | plt.axis("equal") 42 | plt.pause(1.0) 43 | 44 | inds, error = nearest_neighbor_assosiation(ppoints, cpoints) 45 | Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints) 46 | 47 | # update current points 48 | cpoints = (Rt @ cpoints) + Tt[:, np.newaxis] 49 | 50 | H = update_homogeneous_matrix(H, Rt, Tt) 51 | 52 | dError = abs(preError - error) 53 | preError = error 54 | print("Residual:", error) 55 | 56 | if dError <= EPS: 57 | print("Converge", error, dError, count) 58 | break 59 | elif MAXITER <= count: 60 | print("Not Converge...", error, dError, count) 61 | break 62 | 63 | R = np.array(H[0:2, 0:2]) 64 | T = np.array(H[0:2, 2]) 65 | 66 | return R, T 67 | 68 | 69 | def update_homogeneous_matrix(Hin, R, T): 70 | 71 | H = np.zeros((3, 3)) 72 | 73 | H[0, 0] = R[0, 0] 74 | H[1, 0] = R[1, 0] 75 | H[0, 1] = R[0, 1] 76 | H[1, 1] = R[1, 1] 77 | H[2, 2] = 1.0 78 | 79 | H[0, 2] = T[0] 80 | H[1, 2] = T[1] 81 | 82 | if Hin is None: 83 | return H 84 | else: 85 | return Hin @ H 86 | 87 | 88 | def nearest_neighbor_assosiation(ppoints, cpoints): 89 | 90 | # calc the sum of residual errors 91 | dcpoints = ppoints - cpoints 92 | d = np.linalg.norm(dcpoints, axis=0) 93 | error = sum(d) 94 | 95 | # calc index with nearest neighbor assosiation 96 | inds = [] 97 | for i in range(cpoints.shape[1]): 98 | minid = -1 99 | mind = float("inf") 100 | for ii in range(ppoints.shape[1]): 101 | d = np.linalg.norm(ppoints[:, ii] - cpoints[:, i]) 102 | 103 | if mind >= d: 104 | mind = d 105 | minid = ii 106 | 107 | inds.append(minid) 108 | 109 | return inds, error 110 | 111 | 112 | def SVD_motion_estimation(ppoints, cpoints): 113 | 114 | pm = np.mean(ppoints, axis=1) 115 | cm = np.mean(cpoints, axis=1) 116 | 117 | pshift = ppoints - pm[:, np.newaxis] 118 | cshift = cpoints - cm[:, np.newaxis] 119 | 120 | W = cshift @ pshift.T 121 | u, s, vh = np.linalg.svd(W) 122 | 123 | R = (u @ vh).T 124 | t = pm - (R @ cm) 125 | 126 | return R, t 127 | 128 | 129 | def main(): 130 | print(__file__ + " start!!") 131 | 132 | # simulation parameters 133 | nPoint = 10 134 | fieldLength = 50.0 135 | motion = [0.5, 2.0, np.deg2rad(-10.0)] # movement [x[m],y[m],yaw[deg]] 136 | 137 | nsim = 3 # number of simulation 138 | 139 | for _ in range(nsim): 140 | 141 | # previous points 142 | px = (np.random.rand(nPoint) - 0.5) * fieldLength 143 | py = (np.random.rand(nPoint) - 0.5) * fieldLength 144 | ppoints = np.vstack((px, py)) 145 | 146 | # current points 147 | cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0] 148 | for (x, y) in zip(px, py)] 149 | cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1] 150 | for (x, y) in zip(px, py)] 151 | cpoints = np.vstack((cx, cy)) 152 | 153 | R, T = ICP_matching(ppoints, cpoints) 154 | print("R:", R) 155 | print("T:", T) 156 | 157 | 158 | if __name__ == '__main__': 159 | main() -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-slate 2 | show_downloads: true 3 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | environment: 2 | global: 3 | # SDK v7.0 MSVC Express 2008's SetEnv.cmd script will fail if the 4 | # /E:ON and /V:ON options are not enabled in the batch script intepreter 5 | # See: http://stackoverflow.com/a/13751649/163740 6 | CMD_IN_ENV: "cmd /E:ON /V:ON /C .\\appveyor\\run_with_env.cmd" 7 | 8 | 9 | matrix: 10 | - PYTHON_VERSION: 3.6 11 | MINICONDA: "C:\\Miniconda36-x64" 12 | 13 | init: 14 | - "ECHO %MINICONDA% %PYTHON_VERSION% %PYTHON_ARCH%" 15 | 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 | - ECHO "Installed SDKs:" 31 | - ps: "ls \"C:/Program Files/Microsoft SDKs/Windows\"" 32 | 33 | # Prepend newly installed Python to the PATH of this build (this cannot be 34 | # done from inside the powershell script as it would require to restart 35 | # the parent CMD process). 36 | - "SET PATH=%MINICONDA%;%MINICONDA%\\Scripts;%PATH%" 37 | - conda config --set always_yes yes --set changeps1 no 38 | - conda update -q conda 39 | - conda info -a 40 | - "python -m pip install --upgrade pip" 41 | - "python -m pip install numpy" 42 | - conda env create -f C:\\projects\pythonrobotics\environment.yml 43 | - activate python_robotics 44 | 45 | # Check that we have the expected version and architecture for Python 46 | - "python --version" 47 | - "python -c \"import struct; print(struct.calcsize('P') * 8)\"" 48 | 49 | build: off 50 | 51 | test_script: 52 | - "python -Wignore -m unittest discover tests" 53 | -------------------------------------------------------------------------------- /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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/_static/.gitkeep -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # Configuration file for the Sphinx documentation builder. 4 | # 5 | # This file does only contain a selection of the most common options. For a 6 | # full list see the documentation: 7 | # http://www.sphinx-doc.org/en/master/config 8 | 9 | # -- Path setup -------------------------------------------------------------- 10 | 11 | # If extensions (or modules to document with autodoc) are in another directory, 12 | # add these directories to sys.path here. If the directory is relative to the 13 | # documentation root, use os.path.abspath to make it absolute, like shown here. 14 | # 15 | import os 16 | # import sys 17 | # sys.path.insert(0, os.path.abspath('.')) 18 | 19 | 20 | # -- Project information ----------------------------------------------------- 21 | 22 | project = 'PythonRobotics' 23 | copyright = '2018, Atsushi Sakai' 24 | author = 'Atsushi Sakai' 25 | 26 | # The short X.Y version 27 | version = '' 28 | # The full version, including alpha/beta/rc tags 29 | release = '' 30 | 31 | 32 | # -- General configuration --------------------------------------------------- 33 | 34 | # If your documentation needs a minimal Sphinx version, state it here. 35 | # 36 | # needs_sphinx = '1.0' 37 | 38 | # Add any Sphinx extension module names here, as strings. They can be 39 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 40 | # ones. 41 | extensions = [ 42 | 'sphinx.ext.autodoc', 43 | 'sphinx.ext.mathjax', 44 | 'sphinx.ext.viewcode', 45 | 'sphinx.ext.viewcode', 46 | ] 47 | 48 | # Add any paths that contain templates here, relative to this directory. 49 | templates_path = ['_templates'] 50 | 51 | # The suffix(es) of source filenames. 52 | # You can specify multiple suffix as a list of string: 53 | # 54 | # source_suffix = ['.rst', '.md'] 55 | source_suffix = '.rst' 56 | 57 | # The master toctree document. 58 | master_doc = 'index' 59 | 60 | # The language for content autogenerated by Sphinx. Refer to documentation 61 | # for a list of supported languages. 62 | # 63 | # This is also used if you do content translation via gettext catalogs. 64 | # Usually you set "language" from the command line for these cases. 65 | language = None 66 | 67 | # List of patterns, relative to source directory, that match files and 68 | # directories to ignore when looking for source files. 69 | # This pattern also affects html_static_path and html_extra_path . 70 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 71 | 72 | # The name of the Pygments (syntax highlighting) style to use. 73 | pygments_style = 'sphinx' 74 | 75 | 76 | # -- Options for HTML output ------------------------------------------------- 77 | 78 | # The theme to use for HTML and HTML Help pages. See the documentation for 79 | # a list of builtin themes. 80 | # 81 | # Fix for read the docs 82 | on_rtd = os.environ.get('READTHEDOCS') == 'True' 83 | if on_rtd: 84 | html_theme = 'default' 85 | else: 86 | html_theme = 'sphinx_rtd_theme' 87 | 88 | # Theme options are theme-specific and customize the look and feel of a theme 89 | # further. For a list of options available for each theme, see the 90 | # documentation. 91 | # 92 | html_logo = '../icon.png' 93 | html_theme_options = { 94 | 'display_version': False, 95 | } 96 | 97 | # Add any paths that contain custom static files (such as style sheets) here, 98 | # relative to this directory. They are copied after the builtin static files, 99 | # so a file named "default.css" will overwrite the builtin "default.css". 100 | html_static_path = ['_static'] 101 | 102 | # Custom sidebar templates, must be a dictionary that maps document names 103 | # to template names. 104 | # 105 | # The default sidebars (for documents that don't match any pattern) are 106 | # defined by theme itself. Builtin themes are using these templates by 107 | # default: ``['localtoc.html', 'relations.html', 'sourcelink.html', 108 | # 'searchbox.html']``. 109 | # 110 | # html_sidebars = {} 111 | 112 | 113 | # -- Options for HTMLHelp output --------------------------------------------- 114 | 115 | # Output file base name for HTML help builder. 116 | htmlhelp_basename = 'PythonRoboticsdoc' 117 | 118 | 119 | # -- Options for LaTeX output ------------------------------------------------ 120 | 121 | latex_elements = { 122 | # The paper size ('letterpaper' or 'a4paper'). 123 | # 124 | # 'papersize': 'letterpaper', 125 | 126 | # The font size ('10pt', '11pt' or '12pt'). 127 | # 128 | # 'pointsize': '10pt', 129 | 130 | # Additional stuff for the LaTeX preamble. 131 | # 132 | # 'preamble': '', 133 | 134 | # Latex figure (float) alignment 135 | # 136 | # 'figure_align': 'htbp', 137 | } 138 | 139 | # Grouping the document tree into LaTeX files. List of tuples 140 | # (source start file, target name, title, 141 | # author, documentclass [howto, manual, or own class]). 142 | latex_documents = [ 143 | (master_doc, 'PythonRobotics.tex', 'PythonRobotics Documentation', 144 | 'Atsushi Sakai', 'manual'), 145 | ] 146 | 147 | 148 | # -- Options for manual page output ------------------------------------------ 149 | 150 | # One entry per manual page. List of tuples 151 | # (source start file, name, description, authors, manual section). 152 | man_pages = [ 153 | (master_doc, 'pythonrobotics', 'PythonRobotics Documentation', 154 | [author], 1) 155 | ] 156 | 157 | 158 | # -- Options for Texinfo output ---------------------------------------------- 159 | 160 | # Grouping the document tree into Texinfo files. List of tuples 161 | # (source start file, target name, title, author, 162 | # dir menu entry, description, category) 163 | texinfo_documents = [ 164 | (master_doc, 'PythonRobotics', 'PythonRobotics Documentation', 165 | author, 'PythonRobotics', 'One line description of project.', 166 | 'Miscellaneous'), 167 | ] 168 | 169 | 170 | # -- Extension configuration ------------------------------------------------- 171 | -------------------------------------------------------------------------------- /docs/getting_started.rst: -------------------------------------------------------------------------------- 1 | .. _getting_started: 2 | 3 | Getting Started 4 | =============== 5 | 6 | Requirements 7 | ------------- 8 | 9 | - Python 3.6.x 10 | - numpy 11 | - scipy 12 | - matplotlib 13 | - pandas 14 | - `cvxpy`_ 15 | 16 | .. _cvxpy: http://www.cvxpy.org/en/latest/ 17 | 18 | 19 | How to use 20 | ---------- 21 | 22 | 1. Install the required libraries. You can use environment.yml with 23 | conda command. 24 | 25 | 2. Clone this repo. 26 | 27 | 3. Execute python script in each directory. 28 | 29 | 4. Add star to this repo if you like it 😃. 30 | 31 | -------------------------------------------------------------------------------- /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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/FastSLAM1_files/FastSLAM1_12_0.png -------------------------------------------------------------------------------- /docs/modules/FastSLAM1_files/FastSLAM1_12_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/FastSLAM1_files/FastSLAM1_12_1.png -------------------------------------------------------------------------------- /docs/modules/FastSLAM1_files/FastSLAM1_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/FastSLAM1_files/FastSLAM1_1_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png -------------------------------------------------------------------------------- /docs/modules/Model_predictive_speed_and_steering_control.rst: -------------------------------------------------------------------------------- 1 | 2 | Model predictive speed and steering control 3 | ------------------------------------------- 4 | 5 | .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif?raw=true 6 | :alt: Model predictive speed and steering control 7 | 8 | Model predictive speed and steering control 9 | 10 | code: 11 | 12 | `PythonRobotics/model_predictive_speed_and_steer_control.py at master · 13 | AtsushiSakai/PythonRobotics `__ 14 | 15 | This is a path tracking simulation using model predictive control (MPC). 16 | 17 | The MPC controller controls vehicle speed and steering base on 18 | linealized model. 19 | 20 | This code uses cvxpy as an optimization modeling tool. 21 | 22 | - `Welcome to CVXPY 1.0 — CVXPY 1.0.6 23 | documentation `__ 24 | 25 | MPC modeling 26 | ~~~~~~~~~~~~ 27 | 28 | State vector is: 29 | 30 | .. math:: z = [x, y, v,\phi] 31 | 32 | x: x-position, y:y-position, v:velocity, φ: yaw angle 33 | 34 | Input vector is: 35 | 36 | .. math:: u = [a, \delta] 37 | 38 | a: accellation, δ: steering angle 39 | 40 | The MPC cotroller minimize this cost function for path tracking: 41 | 42 | .. math:: min\ Q_f(z_{T,ref}-z_{T})^2+Q\Sigma({z_{t,ref}-z_{t}})^2+R\Sigma{u_t}^2+R_d\Sigma({u_{t+1}-u_{t}})^2 43 | 44 | z_ref come from target path and speed. 45 | 46 | subject to: 47 | 48 | - Linearlied vehicle model 49 | 50 | .. math:: z_{t+1}=Az_t+Bu+C 51 | 52 | - Maximum steering speed 53 | 54 | .. math:: |u_{t+1}-u_{t}|`__ 77 | 78 | Vehicle model linearization 79 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~ 80 | 81 | Vehicle model is 82 | 83 | .. math:: \dot{x} = vcos(\phi) 84 | 85 | .. math:: \dot{y} = vsin((\phi) 86 | 87 | .. math:: \dot{v} = a 88 | 89 | .. math:: \dot{\phi} = \frac{vtan(\delta)}{L} 90 | 91 | ODE is 92 | 93 | .. math:: \dot{z} =\frac{\partial }{\partial z} z = f(z, u) = A'z+B'u 94 | 95 | where 96 | 97 | :math:`\begin{equation*} A' = \begin{bmatrix} \frac{\partial }{\partial x}vcos(\phi) & \frac{\partial }{\partial y}vcos(\phi) & \frac{\partial }{\partial v}vcos(\phi) & \frac{\partial }{\partial \phi}vcos(\phi)\\ \frac{\partial }{\partial x}vsin(\phi) & \frac{\partial }{\partial y}vsin(\phi) & \frac{\partial }{\partial v}vsin(\phi) & \frac{\partial }{\partial \phi}vsin(\phi)\\ \frac{\partial }{\partial x}a& \frac{\partial }{\partial y}a& \frac{\partial }{\partial v}a& \frac{\partial }{\partial \phi}a\\ \frac{\partial }{\partial x}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial y}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial v}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ 　= \begin{bmatrix} 0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\ 0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\ 0 & 0 & 0 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\ \end{bmatrix} \end{equation*}` 98 | 99 | :math:`\begin{equation*} B' = \begin{bmatrix} \frac{\partial }{\partial a}vcos(\phi) & \frac{\partial }{\partial \delta}vcos(\phi)\\ \frac{\partial }{\partial a}vsin(\phi) & \frac{\partial }{\partial \delta}vsin(\phi)\\ \frac{\partial }{\partial a}a & \frac{\partial }{\partial \delta}a\\ \frac{\partial }{\partial a}\frac{vtan(\delta)}{L} & \frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ 　= \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\ \end{bmatrix} \end{equation*}` 100 | 101 | You can get a discrete-time mode with Forward Euler Discretization with 102 | sampling time dt. 103 | 104 | .. math:: z_{k+1}=z_k+f(z_k,u_k)dt 105 | 106 | Using first degree Tayer expantion around zbar and ubar 107 | 108 | .. math:: z_{k+1}=z_k+(f(\bar{z},\bar{u})+A'z_k+B'u_k-A'\bar{z}-B'\bar{u})dt 109 | 110 | .. math:: z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt 111 | 112 | So, 113 | 114 | .. math:: z_{k+1}=Az_k+Bu_k +C 115 | 116 | where, 117 | 118 | :math:`\begin{equation*} A = (I + dtA')\\ = \begin{bmatrix} 1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\ 0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\ \end{bmatrix} \end{equation*}` 119 | 120 | :math:`\begin{equation*} B = dtB'\\ = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ dt & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\ \end{bmatrix} \end{equation*}` 121 | 122 | :math:`\begin{equation*} C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\ = dt( \begin{bmatrix} \bar{v}cos(\bar{\phi})\\ \bar{v}sin(\bar{\phi}) \\ \bar{a}\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} \bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\ \bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\ 0\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} 0\\ 0 \\ \bar{a}\\ \frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\ \end{bmatrix} )\\ = \begin{bmatrix} \bar{v}sin(\bar{\phi})\bar{\phi}dt\\ -\bar{v}cos(\bar{\phi})\bar{\phi}dt\\ 0\\ -\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\ \end{bmatrix} \end{equation*}` 123 | 124 | This equation is implemented at 125 | 126 | `PythonRobotics/model_predictive_speed_and_steer_control.py at 127 | eb6d1cbe6fc90c7be9210bf153b3a04f177cc138 · 128 | AtsushiSakai/PythonRobotics `__ 129 | 130 | Reference 131 | ~~~~~~~~~ 132 | 133 | - `Vehicle Dynamics and Control \| Rajesh Rajamani \| 134 | Springer `__ 135 | 136 | - `MPC Course Material - MPC Lab @ 137 | UC-Berkeley `__ 138 | -------------------------------------------------------------------------------- /docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/cgmres_nmpc_files/cgmres_nmpc_1_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_2_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/cgmres_nmpc_files/cgmres_nmpc_2_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_3_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/docs/modules/cgmres_nmpc_files/cgmres_nmpc_3_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_4_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/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==3.6 4 | - matplotlib 5 | - scipy 6 | - numpy 7 | - pandas 8 | - pip: 9 | - cvxpy 10 | -------------------------------------------------------------------------------- /icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/icon.png -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | pandas 3 | scipy 4 | matplotlib 5 | cvxpy 6 | -------------------------------------------------------------------------------- /runtests.sh: -------------------------------------------------------------------------------- 1 | echo "Run test suites! " 2 | #python -m unittest discover tests 3 | #python -Wignore -m unittest discover tests #ignore warning 4 | coverage run -m unittest discover tests # generate coverage file 5 | -------------------------------------------------------------------------------- /tests/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zippen-Huang/PythonRobotics-/55e847c8d3c34054345f301e2e586d7bbdbf3da7/tests/.gitkeep -------------------------------------------------------------------------------- /tests/test_LQR_planner.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./PathPlanning/LQRPlanner") 5 | 6 | from PathPlanning.LQRPlanner import LQRplanner as m 7 | 8 | print(__file__) 9 | 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | -------------------------------------------------------------------------------- /tests/test_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 | 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/ClosedLoopRRTStar/") 7 | try: 8 | from PathPlanning.ClosedLoopRRTStar import closed_loop_rrt_star_car 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.main(gx=1.0, gy=0.0, gyaw=0.0, maxIter=5) 21 | 22 | 23 | if __name__ == '__main__': # pragma: no cover 24 | test = Test() 25 | test.test1() 26 | -------------------------------------------------------------------------------- /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 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./AerialNavigation/drone_3d_trajectory_following/") 5 | 6 | from AerialNavigation.drone_3d_trajectory_following import drone_3d_trajectory_following as m 7 | print(__file__) 8 | 9 | 10 | class Test(TestCase): 11 | 12 | def test1(self): 13 | m.show_animation = False 14 | m.main() 15 | -------------------------------------------------------------------------------- /tests/test_dubins_path_planning.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from PathPlanning.DubinsPath import dubins_path_planning 3 | import numpy as np 4 | 5 | 6 | class Test(TestCase): 7 | 8 | def test1(self): 9 | start_x = 1.0 # [m] 10 | start_y = 1.0 # [m] 11 | start_yaw = np.deg2rad(45.0) # [rad] 12 | 13 | end_x = -3.0 # [m] 14 | end_y = -3.0 # [m] 15 | end_yaw = np.deg2rad(-45.0) # [rad] 16 | 17 | curvature = 1.0 18 | 19 | px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning( 20 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) 21 | 22 | assert(abs(px[-1] - end_x) <= 0.1) 23 | assert(abs(py[-1] - end_y) <= 0.1) 24 | assert(abs(pyaw[-1] - end_yaw) <= 0.1) 25 | 26 | def test2(self): 27 | dubins_path_planning.show_animation = False 28 | dubins_path_planning.main() 29 | 30 | def test3(self): 31 | dubins_path_planning.show_animation = False 32 | dubins_path_planning.test() 33 | -------------------------------------------------------------------------------- /tests/test_dynamic_window_approach.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | import os 5 | sys.path.append(os.path.dirname(__file__) + "/../") 6 | try: 7 | from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m 8 | except: 9 | raise 10 | 11 | print(__file__) 12 | 13 | 14 | class Test(TestCase): 15 | 16 | def test1(self): 17 | m.show_animation = False 18 | m.main(gx=1.0, gy=1.0) 19 | 20 | 21 | if __name__ == '__main__': # pragma: no cover 22 | test = Test() 23 | test.test1() 24 | -------------------------------------------------------------------------------- /tests/test_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_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_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_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 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./ArmNavigation/n_joint_arm_to_point_control/") 5 | 6 | from ArmNavigation.n_joint_arm_to_point_control import n_joint_arm_to_point_control as m 7 | print(__file__) 8 | 9 | 10 | class Test(TestCase): 11 | 12 | def test1(self): 13 | m.show_animation = False 14 | m.animation() 15 | -------------------------------------------------------------------------------- /tests/test_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 | 5 | class Test(TestCase): 6 | 7 | def test1(self): 8 | m.show_animation = False 9 | m.main() 10 | -------------------------------------------------------------------------------- /tests/test_rocket_powered_landing.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./AerialNavigation/rocket_powered_landing/") 5 | 6 | from AerialNavigation.rocket_powered_landing import rocket_powered_landing as m 7 | print(__file__) 8 | 9 | 10 | class Test(TestCase): 11 | 12 | def test1(self): 13 | m.show_animation = False 14 | m.main() 15 | -------------------------------------------------------------------------------- /tests/test_rrt.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 PathPlanning.RRT import simple_rrt as m 8 | from PathPlanning.RRT import rrt_with_pathsmoothing as m1 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.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 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 5 | "/../PathPlanning/RRTstar/") 6 | 7 | try: 8 | import rrt_star 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.main() 21 | 22 | 23 | if __name__ == '__main__': # pragma: no cover 24 | test = Test() 25 | test.test1() 26 | print(aa) 27 | -------------------------------------------------------------------------------- /tests/test_rrt_star_dubins.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | import os 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 6 | + "/../PathPlanning/RRTStarDubins/") 7 | 8 | try: 9 | import rrt_star_dubins as m 10 | except: 11 | raise 12 | 13 | print(__file__) 14 | 15 | 16 | class Test(TestCase): 17 | 18 | def test1(self): 19 | m.show_animation = False 20 | m.main() 21 | 22 | 23 | if __name__ == '__main__': # pragma: no cover 24 | test = Test() 25 | test.test1() 26 | -------------------------------------------------------------------------------- /tests/test_rrt_star_reeds_shepp.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | import os 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 6 | + "/../PathPlanning/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(maxIter=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_voronoi_path_planner.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from PathPlanning.VoronoiRoadMap import voronoi_road_map as m 3 | 4 | print(__file__) 5 | 6 | 7 | class Test(TestCase): 8 | 9 | def test1(self): 10 | m.show_animation = False 11 | m.main() 12 | -------------------------------------------------------------------------------- /users_comments.md: -------------------------------------------------------------------------------- 1 | 2 | # Educational users 3 | 4 | This is a list of users who are using PythonRobotics for education. 5 | 6 | If you found other users, please make an issue to let me know. 7 | 8 | - [CSCI/ARTI 4530/6530: Introduction to Robotics (Fall 2018), University of Georgia ](http://cobweb.cs.uga.edu/~ramviyas/csci_x530.html) 9 | 10 | # Individual users comments 11 | 12 | These are comments from user's using [![Say Thanks!](https://img.shields.io/badge/Say%20Thanks-!-1EAEDB.svg)](https://saythanks.io/to/AtsushiSakai) 13 | 14 | --- 15 | 16 | Dear AtsushiSakai,
thank you for building this cool repo for robotics.
I am still learning robotics and this will give me a good starting point.
I hope this note gets to you.

17 | 18 | --David Senicic 19 | 20 | --- 21 | 22 | Dear AtsushiSakai,
WE found your project from google when first searching for a rosbag to csv converter. We are a small team in MCity working on Connected (and Automated) vehicles. We are working with an opensource platform that serve as the AI to control the vehicle. Your code and documentation helped us a lot!

Thank you! Have a nice day! 23 | 24 | --Hanlin(Julia) Chen, MCity Apollo team 25 | 26 | --- 27 | 28 | Dear Atsushi Sakai,

With your simplistic descriptions in text and as gifs, i was able to help my students understand easily and effectively, I would be honored to help, in anyway towards this, As a hobbyist have been developing lidar based slam navigation systems from 2011 and its only right to acknowledge and thank you for your contributions. 29 | 30 | --Shiva 31 | 32 | --- 33 | 34 | Dear Atsushi Sakai
I first came across your Matlab repository on ICP and SLAM. The repository for both python and Matlab helped me in understanding the essential concepts of robotics.I am really grateful to you for helping me develop my understanding of the concepts of robotics. 35 | 36 | --Sparsh Garg 37 | 38 | --- 39 | 40 | Dear AtsushiSakai,
Thank you very much for all the example programs related to Robotics 41 | 42 | --Kalyan 43 | 44 | --- 45 | Dear AtsushiSakai,

Thanks for Python Robotics 46 | 47 | --Rebecca Li 48 | 49 | --- 50 | 51 | Thanks alot. 52 | 53 | --Zain 54 | 55 | --- 56 | 57 | Dear AtsushiSakai,

thank you for you code! 58 | 59 | —- Anonymous 60 | 61 | --- 62 | 63 | Thanks! 64 | 65 | --a friend 66 | 67 | --- 68 | 69 | Thanks for the awesome collection of codes! 70 | 71 | --Qi 72 | 73 | --- 74 | 75 | Thank you! 76 | 77 | --huang pingzhong 78 | 79 | --- 80 | 81 | Dear AtsushiSakai,
Thanks a lot for the wonderful collection of projects.It was really useful. I really appreciate your time in maintaing and building this repository.It will be really great if I get a chance to meet you in person sometime.I got really inspired looking at your work.
All the very best for all your future endeavors!
Thanks once again,
82 | 83 | ---Ezhil Bharathi 84 | 85 | --- 86 | 87 | Dear Atsushi Sakai,
Thank you so much for gathering all the useful stuff and good examples of robotics things ! :)
I am very new in this area and want to know more about robotics and SLAM things.
and again, thank you so much :)
88 | 89 | --Tutorgaming 90 | 91 | --- 92 | 93 | Dear AtsushiSakai,
Very excellent work. Thank you for your share. 94 | 95 | --Thomas Yang 96 | 97 | --- 98 | 99 | Dear Atsushi Saka Arigato 🤗🤗 100 | 101 | --Badal Kumar 102 | 103 | --- 104 | 105 | Dear AtsushiSakai,
Thanks for teaching how to implement path planning algorithms in robotics.
106 | 107 | --- 108 | 109 | Your Github page is very useful. I will apply it with cooperative robot. 110 | 111 | --Soloist 112 | 113 | --- 114 | 115 | help me very much thankyou 116 | 117 | --sanchuan 118 | 119 | --- 120 | 121 | Dear AtsushiSakai,
U R so niubility! 122 | 123 | --FangKD 124 | 125 | --- 126 | 127 | thankyou AtsushiSakai! 128 | 129 | --ou minghua 130 | 131 | --- 132 | 133 | Dear AtsushiSakai
Thank You 134 | 135 | --Dogukan Altay 136 | 137 | --- 138 | 139 | so kind of you can you make videos of it. 140 | 141 | --- 142 | 143 | Dear AtshshiSakai,
You are very wise and smart that you created this library for students wanting to learn Probabilistic Robotics and actually perform robot control.
Hats off to you and your work.
I am also reading your book titled : Python Robotics 144 | 145 | --Himanshu 146 | 147 | --- 148 | 149 | 150 | ========= 151 | 152 | # Citations 153 | 154 | 1. B. Blaga, M. Deac, R. W. Y. Al-doori, M. Negru and R. Dǎnescu, "Miniature Autonomous Vehicle Development on Raspberry Pi," 2018 IEEE 14th International Conference on Intelligent Computer Communication and Processing (ICCP), Cluj-Napoca, Romania, 2018, pp. 229-236. 155 | doi: 10.1109/ICCP.2018.8516589 156 | keywords: {Automobiles;Task analysis;Autonomous vehicles;Path planning;Global Positioning System;Cameras;miniature autonomous vehicle;path planning;navigation;parking assist;lane detection and tracking;traffic sign recognition}, 157 | URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8516589&isnumber=8516425 158 | 159 | 2. Peggy (Yuchun) Wang and Caitlin Hogan, "Path Planning with Dynamic Obstacle Avoidance for a Jumping-Enabled Robot", AA228/CS238 class report, Department of Computer Science, Stanford University, URL: https://web.stanford.edu/class/aa228/reports/2018/final113.pdf 160 | 161 | # Others 162 | 163 | - Autonomous Vehicle Readings https://richardkelley.io/readings 164 | 165 | --------------------------------------------------------------------------------