diff --git a/Control/move_to_pose/__init__.py b/Control/move_to_pose/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/PathTracking/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py similarity index 100% rename from PathTracking/move_to_pose/move_to_pose.py rename to Control/move_to_pose/move_to_pose.py diff --git a/docs/modules/control/control_main.rst b/docs/modules/control/control_main.rst index 7e13ab1e..9eeba434 100644 --- a/docs/modules/control/control_main.rst +++ b/docs/modules/control/control_main.rst @@ -4,4 +4,5 @@ Control ================= .. include:: inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.rst +.. include:: move_to_a_pose_control/move_to_a_pose_control.rst diff --git a/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst b/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst new file mode 100644 index 00000000..c5bf15d9 --- /dev/null +++ b/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst @@ -0,0 +1,11 @@ +Move to a pose control +---------------------- + +This is a simulation of moving to a pose control + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif + +Ref: + +- `P. I. Corke, "Robotics, Vision and Control" \| SpringerLink + p102 `__ diff --git a/docs/modules/path_tracking/cgmres_nmpc.rst b/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc.rst similarity index 91% rename from docs/modules/path_tracking/cgmres_nmpc.rst rename to docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc.rst index 6f6a1026..5092fa8e 100644 --- a/docs/modules/path_tracking/cgmres_nmpc.rst +++ b/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc.rst @@ -2,16 +2,16 @@ Nonlinear Model Predictive Control with C-GMRES ----------------------------------------------- -.. image:: cgmres_nmpc_files/cgmres_nmpc_1_0.png +.. image:: cgmres_nmpc/cgmres_nmpc_1_0.png :width: 600px -.. image:: cgmres_nmpc_files/cgmres_nmpc_2_0.png +.. image:: cgmres_nmpc/cgmres_nmpc_2_0.png :width: 600px -.. image:: cgmres_nmpc_files/cgmres_nmpc_3_0.png +.. image:: cgmres_nmpc/cgmres_nmpc_3_0.png :width: 600px -.. image:: cgmres_nmpc_files/cgmres_nmpc_4_0.png +.. image:: cgmres_nmpc/cgmres_nmpc_4_0.png :width: 600px .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/cgmres_nmpc/animation.gif diff --git a/docs/modules/path_tracking/cgmres_nmpc_files/cgmres_nmpc_1_0.png b/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png similarity index 100% rename from docs/modules/path_tracking/cgmres_nmpc_files/cgmres_nmpc_1_0.png rename to docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png diff --git a/docs/modules/path_tracking/cgmres_nmpc_files/cgmres_nmpc_2_0.png b/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png similarity index 100% rename from docs/modules/path_tracking/cgmres_nmpc_files/cgmres_nmpc_2_0.png rename to docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png diff --git a/docs/modules/path_tracking/cgmres_nmpc_files/cgmres_nmpc_3_0.png b/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png similarity index 100% rename from docs/modules/path_tracking/cgmres_nmpc_files/cgmres_nmpc_3_0.png rename to docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png diff --git a/docs/modules/path_tracking/cgmres_nmpc_files/cgmres_nmpc_4_0.png b/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png similarity index 100% rename from docs/modules/path_tracking/cgmres_nmpc_files/cgmres_nmpc_4_0.png rename to docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png diff --git a/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control.rst b/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control.rst new file mode 100644 index 00000000..68ea9c88 --- /dev/null +++ b/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control.rst @@ -0,0 +1,13 @@ +.. _linearquadratic-regulator-(lqr)-speed-and-steering-control: + +Linear–quadratic regulator (LQR) speed and steering control +----------------------------------------------------------- + +Path tracking simulation with LQR speed and steering control. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif + +References: +~~~~~~~~~~~ + +- `Towards fully autonomous driving: Systems and algorithms `__ diff --git a/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control.rst b/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control.rst new file mode 100644 index 00000000..bf6d6b58 --- /dev/null +++ b/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control.rst @@ -0,0 +1,14 @@ +.. _linearquadratic-regulator-(lqr)-steering-control: + +Linear–quadratic regulator (LQR) steering control +------------------------------------------------- + +Path tracking simulation with LQR steering control and PID speed +control. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif + +References: +~~~~~~~~~~~ +- `ApolloAuto/apollo: An open autonomous driving platform `_ + diff --git a/docs/modules/path_tracking/Model_predictive_speed_and_steering_control.rst b/docs/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.rst similarity index 100% rename from docs/modules/path_tracking/Model_predictive_speed_and_steering_control.rst rename to docs/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.rst diff --git a/docs/modules/path_tracking/path_tracking_main.rst b/docs/modules/path_tracking/path_tracking_main.rst index 725e4f04..2a572ef6 100644 --- a/docs/modules/path_tracking/path_tracking_main.rst +++ b/docs/modules/path_tracking/path_tracking_main.rst @@ -3,94 +3,11 @@ Path Tracking ============= -move to a pose control ----------------------- - -This is a simulation of moving to a pose control - -.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif - -Ref: - -- `P. I. Corke, "Robotics, Vision and Control" \| SpringerLink - p102 `__ - -Pure pursuit tracking ---------------------- - -Path tracking simulation with pure pursuit steering control and PID -speed control. - -.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/pure_pursuit/animation.gif - -The red line is a target course, the green cross means the target point -for pure pursuit control, the blue line is the tracking. - -Ref: - -- `A Survey of Motion Planning and Control Techniques for Self-driving - Urban Vehicles `__ - -Stanley control ---------------- - -Path tracking simulation with Stanley steering control and PID speed -control. - -.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif - -Ref: - -- `Stanley: The robot that won the DARPA grand - challenge `__ - -- `Automatic Steering Methods for Autonomous Automobile Path - Tracking `__ - -Rear wheel feedback control ---------------------------- - -Path tracking simulation with rear wheel feedback steering control and -PID speed control. - -.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif - -Ref: - -- `A Survey of Motion Planning and Control Techniques for Self-driving - Urban Vehicles `__ - -.. _linearquadratic-regulator-(lqr)-steering-control: - -Linear–quadratic regulator (LQR) steering control -------------------------------------------------- - -Path tracking simulation with LQR steering control and PID speed -control. - -.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif - -Ref: - -- `ApolloAuto/apollo: An open autonomous driving - platform `__ - -.. _linearquadratic-regulator-(lqr)-speed-and-steering-control: - -Linear–quadratic regulator (LQR) speed and steering control ------------------------------------------------------------ - -Path tracking simulation with LQR speed and steering control. - -.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif - -Ref: - -- `Towards fully autonomous driving: Systems and algorithms - IEEE - Conference - Publication `__ - -.. include:: Model_predictive_speed_and_steering_control.rst - -.. include:: cgmres_nmpc.rst +.. include:: pure_pursuit_tracking/pure_pursuit_tracking.rst +.. include:: stanley_control/stanley_control.rst +.. include:: rear_wheel_feedback_control/rear_wheel_feedback_control.rst +.. include:: lqr_steering_control/lqr_steering_control.rst +.. include:: lqr_speed_and_steering_control/lqr_speed_and_steering_control.rst +.. include:: model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.rst +.. include:: cgmres_nmpc/cgmres_nmpc.rst diff --git a/docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking.rst b/docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking.rst new file mode 100644 index 00000000..5c7bcef8 --- /dev/null +++ b/docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking.rst @@ -0,0 +1,16 @@ +Pure pursuit tracking +--------------------- + +Path tracking simulation with pure pursuit steering control and PID +speed control. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/pure_pursuit/animation.gif + +The red line is a target course, the green cross means the target point +for pure pursuit control, the blue line is the tracking. + +References: +~~~~~~~~~~~ + +- `A Survey of Motion Planning and Control Techniques for Self-driving + Urban Vehicles `_ diff --git a/docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control.rst b/docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control.rst new file mode 100644 index 00000000..70875fdc --- /dev/null +++ b/docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control.rst @@ -0,0 +1,12 @@ +Rear wheel feedback control +--------------------------- + +Path tracking simulation with rear wheel feedback steering control and +PID speed control. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif + +References: +~~~~~~~~~~~ +- `A Survey of Motion Planning and Control Techniques for Self-driving + Urban Vehicles `__ diff --git a/docs/modules/path_tracking/stanley_control/stanley_control.rst b/docs/modules/path_tracking/stanley_control/stanley_control.rst new file mode 100644 index 00000000..fe325b01 --- /dev/null +++ b/docs/modules/path_tracking/stanley_control/stanley_control.rst @@ -0,0 +1,16 @@ +Stanley control +--------------- + +Path tracking simulation with Stanley steering control and PID speed +control. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif + +References: +~~~~~~~~~~~ + +- `Stanley: The robot that won the DARPA grand + challenge `_ + +- `Automatic Steering Methods for Autonomous Automobile Path + Tracking `_ diff --git a/docs/modules/slam/FastSLAM1.rst b/docs/modules/slam/FastSLAM1/FastSLAM1.rst similarity index 97% rename from docs/modules/slam/FastSLAM1.rst rename to docs/modules/slam/FastSLAM1/FastSLAM1.rst index 6e26aef7..f1e9dfd8 100644 --- a/docs/modules/slam/FastSLAM1.rst +++ b/docs/modules/slam/FastSLAM1/FastSLAM1.rst @@ -2,8 +2,7 @@ FastSLAM1.0 ----------- -.. image:: FastSLAM1_files/FastSLAM1_1_0.png - :width: 600px +.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM1/animation.gif @@ -12,6 +11,9 @@ Simulation This is a feature based SLAM example using FastSLAM 1.0. +.. image:: FastSLAM1/FastSLAM1_1_0.png + :width: 600px + The blue line is ground truth, the black line is dead reckoning, the red line is the estimated trajectory with FastSLAM. @@ -20,8 +22,6 @@ The red points are particles of FastSLAM. Black points are landmarks, blue crosses are estimated landmark positions by FastSLAM. -.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM1/animation.gif - :alt: gif Introduction ~~~~~~~~~~~~ @@ -527,19 +527,13 @@ indices -.. image:: FastSLAM1_files/FastSLAM1_12_0.png - - - -.. image:: FastSLAM1_files/FastSLAM1_12_1.png +.. image:: FastSLAM1/FastSLAM1_12_0.png +.. image:: FastSLAM1/FastSLAM1_12_1.png References ~~~~~~~~~~ -- `PROBABILISTIC ROBOTICS`_ +- `PROBABILISTIC ROBOTICS `_ -- `FastSLAM Lecture`_ - -.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/ -.. _FastSLAM Lecture: http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam10-fastslam.pdf +- `FastSLAM Lecture `_ diff --git a/docs/modules/slam/FastSLAM1_files/FastSLAM1_12_0.png b/docs/modules/slam/FastSLAM1/FastSLAM1_12_0.png similarity index 100% rename from docs/modules/slam/FastSLAM1_files/FastSLAM1_12_0.png rename to docs/modules/slam/FastSLAM1/FastSLAM1_12_0.png diff --git a/docs/modules/slam/FastSLAM1_files/FastSLAM1_12_1.png b/docs/modules/slam/FastSLAM1/FastSLAM1_12_1.png similarity index 100% rename from docs/modules/slam/FastSLAM1_files/FastSLAM1_12_1.png rename to docs/modules/slam/FastSLAM1/FastSLAM1_12_1.png diff --git a/docs/modules/slam/FastSLAM1_files/FastSLAM1_1_0.png b/docs/modules/slam/FastSLAM1/FastSLAM1_1_0.png similarity index 100% rename from docs/modules/slam/FastSLAM1_files/FastSLAM1_1_0.png rename to docs/modules/slam/FastSLAM1/FastSLAM1_1_0.png diff --git a/docs/modules/slam/FastSLAM2/FastSLAM2.rst b/docs/modules/slam/FastSLAM2/FastSLAM2.rst new file mode 100644 index 00000000..9e79b496 --- /dev/null +++ b/docs/modules/slam/FastSLAM2/FastSLAM2.rst @@ -0,0 +1,16 @@ +FastSLAM 2.0 +------------ + +This is a feature based SLAM example using FastSLAM 2.0. + +The animation has the same meanings as one of FastSLAM 1.0. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM2/animation.gif + +References +~~~~~~~~~~ + +- `PROBABILISTIC ROBOTICS `_ + +- `SLAM simulations by Tim Bailey `_ + diff --git a/docs/modules/slam/ekf_slam.rst b/docs/modules/slam/ekf_slam/ekf_slam.rst similarity index 98% rename from docs/modules/slam/ekf_slam.rst rename to docs/modules/slam/ekf_slam/ekf_slam.rst index df520fbd..a79c2737 100644 --- a/docs/modules/slam/ekf_slam.rst +++ b/docs/modules/slam/ekf_slam/ekf_slam.rst @@ -8,7 +8,7 @@ line is the estimated trajectory with EKF SLAM. The green crosses are estimated landmarks. -|4| +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/EKFSLAM/animation.gif Simulation ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -581,13 +581,11 @@ reckoning and control functions are passed along here as well. New LM New LM -.. image:: ekf_slam_files/ekf_slam_1_0.png +.. image:: ekf_slam/ekf_slam_1_0.png References: ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -- `PROBABILISTIC ROBOTICS`_ +- `PROBABILISTIC ROBOTICS `_ -.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/ -.. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/EKFSLAM/animation.gif diff --git a/docs/modules/slam/ekf_slam_files/ekf_slam_1_0.png b/docs/modules/slam/ekf_slam/ekf_slam_1_0.png similarity index 100% rename from docs/modules/slam/ekf_slam_files/ekf_slam_1_0.png rename to docs/modules/slam/ekf_slam/ekf_slam_1_0.png diff --git a/docs/modules/slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif b/docs/modules/slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif deleted file mode 100644 index 2fabeaaf..00000000 Binary files a/docs/modules/slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif and /dev/null differ diff --git a/docs/modules/slam/graphSLAM_SE2_example.rst b/docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst similarity index 90% rename from docs/modules/slam/graphSLAM_SE2_example.rst rename to docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst index bcc797ba..ef5d05bf 100644 --- a/docs/modules/slam/graphSLAM_SE2_example.rst +++ b/docs/modules/slam/graph_slam/graphSLAM_SE2_example.rst @@ -44,7 +44,7 @@ The Dataset -.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png +.. image:: graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png Each edge in this dataset is a constraint that compares the measured @@ -122,7 +122,7 @@ dataset and plot them. -.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png +.. image:: graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png .. code:: ipython3 @@ -131,7 +131,7 @@ dataset and plot them. -.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png +.. image:: graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png Optimization @@ -165,8 +165,7 @@ different data sources into a single optimization problem. 6 215.8405 -0.000000 -.. figure:: graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif - :alt: Graph_SLAM_optimization.gif +.. figure:: graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif .. code:: ipython3 @@ -174,7 +173,7 @@ different data sources into a single optimization problem. -.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png +.. image:: graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png .. code:: ipython3 @@ -196,7 +195,7 @@ different data sources into a single optimization problem. -.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png +.. image:: graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png .. code:: ipython3 @@ -205,5 +204,5 @@ different data sources into a single optimization problem. -.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png +.. image:: graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png diff --git a/docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png b/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png similarity index 100% rename from docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png rename to docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png diff --git a/docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png b/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png similarity index 100% rename from docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png rename to docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png diff --git a/docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png b/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png similarity index 100% rename from docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png rename to docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png diff --git a/docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png b/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png similarity index 100% rename from docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png rename to docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png diff --git a/docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png b/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png similarity index 100% rename from docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png rename to docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png diff --git a/docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png b/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png similarity index 100% rename from docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png rename to docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png diff --git a/docs/modules/slam/graphSLAM_doc.rst b/docs/modules/slam/graph_slam/graphSLAM_doc.rst similarity index 95% rename from docs/modules/slam/graphSLAM_doc.rst rename to docs/modules/slam/graph_slam/graphSLAM_doc.rst index b17c3b18..4367f770 100644 --- a/docs/modules/slam/graphSLAM_doc.rst +++ b/docs/modules/slam/graph_slam/graphSLAM_doc.rst @@ -142,7 +142,7 @@ created based on the information of the motion and the observation. -.. image:: graphSLAM_doc_files/graphSLAM_doc_2_0.png +.. image:: graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png .. parsed-literal:: @@ -157,7 +157,7 @@ created based on the information of the motion and the observation. -.. image:: graphSLAM_doc_files/graphSLAM_doc_2_2.png +.. image:: graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png In particular, the tasks are split into 2 parts, graph construction, and @@ -289,7 +289,7 @@ robot with 3DoF, namely, :math:`[x, y, \theta]^T` -.. image:: graphSLAM_doc_files/graphSLAM_doc_4_0.png +.. image:: graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png .. code:: ipython3 @@ -420,7 +420,7 @@ zero since :math:`x_j + d_j cos(\psi_j + \theta_j)` should equal -.. image:: graphSLAM_doc_files/graphSLAM_doc_9_1.png +.. image:: graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png Since the constraints equations derived before are non-linear, @@ -494,12 +494,9 @@ Similarly, :math:`B = \frac{\partial e_{ij}}{\partial \boldsymbol{x}_j}` -.. image:: graphSLAM_doc_files/graphSLAM_doc_11_1.png - - - -.. image:: graphSLAM_doc_files/graphSLAM_doc_11_2.png +.. image:: graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png +.. image:: graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png .. code:: ipython3 @@ -546,11 +543,11 @@ Similarly, :math:`B = \frac{\partial e_{ij}}{\partial \boldsymbol{x}_j}` The references: ^^^^^^^^^^^^^^^ -- http://robots.stanford.edu/papers/thrun.graphslam.pdf +- `The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures `_ -- http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/slides/16-graph-slam.pdf +- `Introduction to Mobile Robotics Graph-Based SLAM `_ -- http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf +- `A Tutorial on Graph-Based SLAM `_ N.B. An additional step is required that uses the estimated path to update the belief regarding the map. diff --git a/docs/modules/slam/graphSLAM_formulation.rst b/docs/modules/slam/graph_slam/graphSLAM_formulation.rst similarity index 97% rename from docs/modules/slam/graphSLAM_formulation.rst rename to docs/modules/slam/graph_slam/graphSLAM_formulation.rst index c673bdcb..43f007e7 100644 --- a/docs/modules/slam/graphSLAM_formulation.rst +++ b/docs/modules/slam/graph_slam/graphSLAM_formulation.rst @@ -213,4 +213,6 @@ Using this notation, we obtain the optimal update as We apply this update to the poses via :eq:`update` and repeat until convergence. -.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/ +.. [blanco2010tutorial] Blanco, J.-L.A tutorial onSE(3) transformation parameterization and on-manifold optimization.University of Malaga, Tech. Rep 3(2010) +.. [grisetti2010tutorial] Grisetti, G., Kummerle, R., Stachniss, C., and Burgard, W.A tutorial on graph-based SLAM.IEEE Intelligent Transportation Systems Magazine 2, 4 (2010), 31–43. + diff --git a/docs/modules/slam/graph_slam/graph_slam.rst b/docs/modules/slam/graph_slam/graph_slam.rst new file mode 100644 index 00000000..a1248a01 --- /dev/null +++ b/docs/modules/slam/graph_slam/graph_slam.rst @@ -0,0 +1,24 @@ +Graph based SLAM +---------------- + +This is a graph based SLAM example. + +The blue line is ground truth. + +The black line is dead reckoning. + +The red line is the estimated trajectory with Graph based SLAM. + +The black stars are landmarks for graph edge generation. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/animation.gif + +.. include:: graph_slam/graphSLAM_doc.rst +.. include:: graph_slam/graphSLAM_formulation.rst +.. include:: graph_slam/graphSLAM_SE2_example.rst + +References: +~~~~~~~~~~~ + +- `A Tutorial on Graph-Based SLAM `_ + diff --git a/docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching.rst b/docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching.rst new file mode 100644 index 00000000..a30b1fc9 --- /dev/null +++ b/docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching.rst @@ -0,0 +1,16 @@ +.. _iterative-closest-point-(icp)-matching: + +Iterative Closest Point (ICP) Matching +-------------------------------------- + +This is a 2D ICP matching example with singular value decomposition. + +It can calculate a rotation matrix and a translation vector between +points to points. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif + +References +~~~~~~~~~~ + +- `Introduction to Mobile Robotics: Iterative Closest Point Algorithm `_ diff --git a/docs/modules/slam/slam_main.rst b/docs/modules/slam/slam_main.rst index 41210fee..f4d10feb 100644 --- a/docs/modules/slam/slam_main.rst +++ b/docs/modules/slam/slam_main.rst @@ -5,77 +5,9 @@ SLAM Simultaneous Localization and Mapping(SLAM) examples -.. _iterative-closest-point-(icp)-matching: +.. include:: iterative_closest_point_matching/iterative_closest_point_matching.rst +.. include:: ekf_slam/ekf_slam.rst +.. include:: FastSLAM1/FastSLAM1.rst +.. include:: FastSLAM2/FastSLAM2.rst +.. include:: graph_slam/graph_slam.rst -Iterative Closest Point (ICP) Matching --------------------------------------- - -This is a 2D ICP matching example with singular value decomposition. - -It can calculate a rotation matrix and a translation vector between -points to points. - -|3| - -Ref: - -- `Introduction to Mobile Robotics: Iterative Closest Point Algorithm`_ - - -.. include:: ekf_slam.rst - - -.. include:: FastSLAM1.rst - -FastSLAM 2.0 ------------- - -This is a feature based SLAM example using FastSLAM 2.0. - -The animation has the same meanings as one of FastSLAM 1.0. - -|6| - -References -~~~~~~~~~~ - -- `PROBABILISTIC ROBOTICS`_ - -- `SLAM simulations by Tim Bailey`_ - -Graph based SLAM ----------------- - -This is a graph based SLAM example. - -The blue line is ground truth. - -The black line is dead reckoning. - -The red line is the estimated trajectory with Graph based SLAM. - -The black stars are landmarks for graph edge generation. - -|7| - -.. include:: graphSLAM_doc.rst -.. include:: graphSLAM_formulation.rst -.. include:: graphSLAM_SE2_example.rst - -Ref: - -- `A Tutorial on Graph-Based SLAM`_ - -.. _`Introduction to Mobile Robotics: Iterative Closest Point Algorithm`: https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf -.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/ -.. _SLAM simulations by Tim Bailey: http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm -.. _A Tutorial on Graph-Based SLAM: http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf -.. _FastSLAM Lecture: http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam10-fastslam.pdf - -.. [blanco2010tutorial] Blanco, J.-L.A tutorial onSE(3) transformation parameterization and on-manifold optimization.University of Malaga, Tech. Rep 3(2010) -.. [grisetti2010tutorial] Grisetti, G., Kummerle, R., Stachniss, C., and Burgard, W.A tutorial on graph-based SLAM.IEEE Intelligent Transportation Systems Magazine 2, 4 (2010), 31–43. - -.. |3| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif -.. |5| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM1/animation.gif -.. |6| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM2/animation.gif -.. |7| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/animation.gif diff --git a/tests/test_move_to_pose.py b/tests/test_move_to_pose.py index c6a6b8bc..05cfa269 100644 --- a/tests/test_move_to_pose.py +++ b/tests/test_move_to_pose.py @@ -1,5 +1,5 @@ import conftest # Add root path to sys.path -from PathTracking.move_to_pose import move_to_pose as m +from Control.move_to_pose import move_to_pose as m def test_1():