├── .gitattributes ├── .gitignore ├── LICENSE ├── README.md ├── README.tex.md ├── check_dependencies.m ├── simulator_files ├── agent │ ├── turtlebot_LLC.m │ ├── turtlebot_PD_LLC.m │ ├── turtlebot_agent.m │ └── turtlebot_pursuit_LLC.m └── planner │ ├── my_turtlebot_RTD_planner.m │ ├── turtlebot_RRT_planner.m │ ├── turtlebot_RRT_star_planner.m │ ├── turtlebot_RTD_planner_static.m │ └── turtlebot_RTD_planner_static_subclass.m ├── step_1_desired_trajectories ├── README.md ├── README.tex.md ├── data │ └── turtlebot_timing.mat ├── examples │ ├── step_1_ex_1_desired_trajectory.m │ ├── step_1_ex_2_trajectory_tracking.m │ └── step_1_ex_3_braking_trajectory.m ├── functions │ ├── get_k_from_w_and_v.m │ ├── get_t_f_from_v_0.m │ ├── get_t_stop_from_v.m │ ├── get_turtlebot_braking_scale_at_time.m │ ├── make_turtlebot_braking_trajectory.m │ ├── make_turtlebot_desired_trajectory.m │ └── turtlebot_trajectory_producing_model.m ├── images │ ├── step_1.5_img_1.png │ ├── step_1.5_img_2.png │ ├── step_1_ex_1_img_1.png │ ├── step_1_ex_2_img_1.png │ └── step_1_ex_3_img_1.png ├── scripts │ ├── step_1_inspect_braking_distance_vs_initial_speed.m │ └── step_1_validate_t_f.m └── tex │ ├── 004273cb240d969e5bce673799ff9da5.svg │ ├── 160b55943c56feaa02bd7a2287e2b41f.svg │ ├── 27e556cf3caa0673ac49a8f0de3c73ca.svg │ ├── 2d7a24fdab5e4f1b25998b6dde017ede.svg │ ├── 2db35060f2b6f146752157657cfb5d5a.svg │ ├── 332cc365a4987aacce0ead01b8bdcc0b.svg │ ├── 402ff5fdff7e663170c8b257765739e7.svg │ ├── 46e582aec9cd4cb9b0aef679b516a387.svg │ ├── 477a717e18587a5e8605780ca167c322.svg │ ├── 4a7d1212c2662d4bee8bf051d1f80f20.svg │ ├── 506ecb28d73537baca60cb655c20dc3f.svg │ ├── 59edb4731438111eb9b889285d97715b.svg │ ├── 5ae1e561ec81f97d93ed9df0f76cab27.svg │ ├── 63e302efe61ebe533fe9bf97f51c439a.svg │ ├── 654de0324269221927923d504c5b6713.svg │ ├── 6c4adbc36120d62b98deef2a20d5d303.svg │ ├── 6d46dad425660c882873584aa5b756b3.svg │ ├── 7392a8cd69b275fa1798ef94c839d2e0.svg │ ├── 742bf637e0c64b5b92d7f44f4570186a.svg │ ├── 83c7347fe075f7e3998685dff1f80070.svg │ ├── 8dd6d948c7b7a6bef7250f4b3437d094.svg │ ├── 8f1dd0bcbdbe44be17955936015724d7.svg │ ├── 91644a9f56940d6f8d879296fd1a54ba.svg │ ├── a362f086bf1a268c373d882b17f36d6b.svg │ ├── a417f6050fede18cdce43b0f0bc57919.svg │ ├── a4393394a1393d0cef2a056237cdbae4.svg │ ├── a58aa8b39e323167ed70b4ca0562c7f8.svg │ ├── a8ebf8c468236800b8ed78d42ddbfa57.svg │ ├── a903f939f6fb649debe1561ecbb18c66.svg │ ├── aa90653a26bc63b138fb304972d81589.svg │ ├── ae4fb5973f393577570881fc24fc2054.svg │ ├── af6c80e33a666ccbd4ad31540ac57c74.svg │ ├── b58de9e05bf7116bb3dc3829ccdef6f7.svg │ ├── b7fd75d9e63fb571e34c17e52105fb14.svg │ ├── bab17f1c90dbcda2fd355c88f6baacb1.svg │ ├── bbe77024f3f059a59d705d15a7827e6c.svg │ ├── bf4645e786baf289adfe68fe608d3e69.svg │ ├── c0213ff9cc036054ae01949a656bca82.svg │ ├── c0fcaca6a19c64c679afd966e48da128.svg │ ├── c3cd2c7b7f2c9024a22d57bd8af387c0.svg │ ├── c4e54ba264a1e2224bc32fc1952f8706.svg │ ├── ce0b32d8f36ea304a17eeaaffd9ff920.svg │ ├── d5ce7af04a5f115d28ff733f2c68ecc4.svg │ ├── deceeaf6940a8c7a5a02373728002b0f.svg │ ├── e18b24c87a7c52fd294215d16b42a437.svg │ ├── e520572846b780b6686ac4a5b2db01fa.svg │ ├── eb4513540706477e80b47eb048eeaa9d.svg │ └── f0490d8095c97a04da6915408d44616b.svg ├── step_2_error_function ├── README.md ├── README.tex.md ├── data │ ├── turtlebot_error_functions_v_0_0.0_to_0.5.mat │ ├── turtlebot_error_functions_v_0_0.5_to_1.0.mat │ └── turtlebot_error_functions_v_0_1.0_to_1.5.mat ├── examples │ ├── step_2_ex_1_tracking_error_single_traj.m │ └── step_2_ex_2_tracking_error_single_init_speed.m ├── images │ ├── step_2.2_img_1.png │ ├── step_2_ex_1_img_1.png │ ├── step_2_ex_1_img_2.png │ └── step_2_ex_2_img_1.png ├── scripts │ ├── step_2_compute_tracking_error_function.m │ └── step_2_inspect_turtlebot_desired_vs_braking_traj.m └── tex │ ├── 01909b179a6e1f6050dd426d4f7195e4.svg │ ├── 0eea379cc2ca8cf230beb920cb2d9e64.svg │ ├── 1a90eb0e181463396ea8d4e475657218.svg │ ├── 1fe5a7300ead2509399189fe2979594e.svg │ ├── 27e556cf3caa0673ac49a8f0de3c73ca.svg │ ├── 332cc365a4987aacce0ead01b8bdcc0b.svg │ ├── 35569fab70bcbeac8b22b2fd3c4533cd.svg │ ├── 3cf4fbd05970446973fc3d9fa3fe3c41.svg │ ├── 3e84cf4a05e7dde39c784cc7a2a632ab.svg │ ├── 408dd2a1a0db44417e1546d7501f069c.svg │ ├── 45f39df0d0bc9f2eb393f4c0f076aa0d.svg │ ├── 469bfc52df7f220df76c05b4b2a9badb.svg │ ├── 4f416c18f7648d12704f5d01199aea2c.svg │ ├── 54dcb7a27315af0a7049460c7fc60e36.svg │ ├── 5ae1e561ec81f97d93ed9df0f76cab27.svg │ ├── 6bac6ec50c01592407695ef84f457232.svg │ ├── 6c4adbc36120d62b98deef2a20d5d303.svg │ ├── 751613a1a4da78db7647a339cbf261c3.svg │ ├── 75aa7badb1a728bd79eb5ac70a314369.svg │ ├── 7fafceb198a08f911de62716356f2a63.svg │ ├── 8bd7e348c9058d16331f871b3632f453.svg │ ├── 8dd6d948c7b7a6bef7250f4b3437d094.svg │ ├── ae4fb5973f393577570881fc24fc2054.svg │ ├── b425862078769aec0d580a5f80bc683d.svg │ ├── c263eac0ba2e77d4cf1a3908e06c213a.svg │ ├── c4700c9a28332f587a2742c1811a32ff.svg │ ├── cbfb1b2a33b28eab8a3e59464768e810.svg │ ├── d0231ad6c4ea2c12b90e4e356a5c0628.svg │ ├── da19af0347f21815ec727c7e4ba63372.svg │ ├── deceeaf6940a8c7a5a02373728002b0f.svg │ ├── df504b9c05ddd4b0cec0f8eee75d215d.svg │ ├── e09e5afacd3ffe83b9366c06552ac2ed.svg │ ├── ee06addff870fbf13ba2fc1625c447e4.svg │ ├── f4102f6671dc35e7ae467aa61bf1bbd3.svg │ └── f75ef0016eb9e3badc61cfc680ce157f.svg ├── step_3_FRS_computation ├── README.md ├── README.tex.md ├── data │ ├── reach_sets │ │ ├── turtlebot_FRS_deg_10_v_0_0.0_to_0.5.mat │ │ ├── turtlebot_FRS_deg_10_v_0_0.5_to_1.0.mat │ │ ├── turtlebot_FRS_deg_10_v_0_1.0_to_1.5.mat │ │ ├── turtlebot_FRS_deg_12_v_0_0.0_to_0.5.mat │ │ ├── turtlebot_FRS_deg_12_v_0_0.5_to_1.0.mat │ │ ├── turtlebot_FRS_deg_12_v_0_1.0_to_1.5.mat │ │ ├── turtlebot_FRS_deg_4_v_0_0.0_to_0.5.mat │ │ ├── turtlebot_FRS_deg_4_v_0_0.5_to_1.0.mat │ │ ├── turtlebot_FRS_deg_4_v_0_1.0_to_1.5.mat │ │ ├── turtlebot_FRS_deg_6_v_0_0.0_to_0.5.mat │ │ ├── turtlebot_FRS_deg_6_v_0_0.5_to_1.0.mat │ │ ├── turtlebot_FRS_deg_6_v_0_1.0_to_1.5.mat │ │ ├── turtlebot_FRS_deg_8_v_0_0.0_to_0.5.mat │ │ ├── turtlebot_FRS_deg_8_v_0_0.5_to_1.0.mat │ │ └── turtlebot_FRS_deg_8_v_0_1.0_to_1.5.mat │ └── scaling │ │ ├── turtlebot_FRS_scaling_v_0_0.0_to_0.5.mat │ │ ├── turtlebot_FRS_scaling_v_0_0.5_to_1.0.mat │ │ └── turtlebot_FRS_scaling_v_0_1.0_to_1.5.mat ├── examples │ ├── step_3_ex_1_FRS_with_fixed_traj_params.m │ ├── step_3_ex_2_visualize_turtlebot_FRS.m │ └── step_3_ex_3_inspect_FRS_conservatism.m ├── functions │ ├── get_FRS_from_v_0.m │ ├── get_v_0_range_from_v_0.m │ ├── plot_turtlebot_FRS_in_world_frame.m │ └── visualize_turtlebot_FRS.m ├── images │ ├── image_1_for_example_7.png │ ├── image_2_for_example_7.png │ ├── image_for_example_6.png │ ├── image_for_example_8.png │ ├── step_3_ex_1_img_1.png │ ├── step_3_ex_2_img_1.png │ └── step_3_ex_3_img_1.png ├── scripts │ ├── step_3_compute_distance_scale.m │ ├── step_3_compute_turtlebot_FRS.m │ └── step_3_verify_distance_scale.m └── tex │ ├── 02365d9a1eafb4fa93ec35acf8c9025d.svg │ ├── 02adb0ec50c9f20dc425f22afae58cdb.svg │ ├── 034d0a6be0424bffe9a6e7ac9236c0f5.svg │ ├── 044056ec7f1f68545fd44d8e6c1a1bd3.svg │ ├── 08926724d3ef194857807025173aaf91.svg │ ├── 0992ba4780d729a0f09b874a1e8fae95.svg │ ├── 190083ef7a1625fbc75f243cffb9c96d.svg │ ├── 1ae6831711d8e1f0131be221eb2de1aa.svg │ ├── 21fd4e8eecd6bdf1a4d3d6bd1fb8d733.svg │ ├── 25a99f7aac484a2091ea7007d2d42cef.svg │ ├── 27e556cf3caa0673ac49a8f0de3c73ca.svg │ ├── 2b2595d381c04d836f219b7837ded4c2.svg │ ├── 2d01c15095f2f5dbf50b0a3c967497a1.svg │ ├── 2db35060f2b6f146752157657cfb5d5a.svg │ ├── 2f118ee06d05f3c2d98361d9c30e38ce.svg │ ├── 332cc365a4987aacce0ead01b8bdcc0b.svg │ ├── 340224300a4ba094f276aad0e7964f95.svg │ ├── 3876431104bfbcad009341427868b85b.svg │ ├── 3aec9ee517c47acbb4dc2bb509d150af.svg │ ├── 3bf25d9d50c08c7ec96446a7abb1c024.svg │ ├── 3cf4fbd05970446973fc3d9fa3fe3c41.svg │ ├── 3d5c283292be654057af73820d289beb.svg │ ├── 477a717e18587a5e8605780ca167c322.svg │ ├── 4b647d9397449f7c2f8099db64815621.svg │ ├── 5396bb939ac02109c36b48c29f7c9ce1.svg │ ├── 5537cda6dac51f6d1e672a0f75f3746e.svg │ ├── 5b51bd2e6f329245d425b8002d7cf942.svg │ ├── 5b5ca9b785667ee30100b8fce0f46933.svg │ ├── 5d0af6d512d3896687daa7411001431e.svg │ ├── 63bb9849783d01d91403bc9a5fea12a2.svg │ ├── 64ddb675e322bf1281650681445f5914.svg │ ├── 699628c77c65481a123e3649944c0d51.svg │ ├── 69a8d98b45d37ba8a111815cbbce31b0.svg │ ├── 6b6744ce28bcf96f11b21c52f6c47c2f.svg │ ├── 6b78aa0639dad385795b3d822af3ce7d.svg │ ├── 6c4adbc36120d62b98deef2a20d5d303.svg │ ├── 6cccb1f776b4ad866a04b52ffc8c7c17.svg │ ├── 6e5c1d28f1dfa35bd2a47b467d0d61d8.svg │ ├── 7392a8cd69b275fa1798ef94c839d2e0.svg │ ├── 7676df17927aeb8cf3c0529814b86278.svg │ ├── 76a9bb39fa5fbab3b219f68d4647c4e0.svg │ ├── 7713ebefc1497d7f70135f405da06c46.svg │ ├── 7b63da949be9d2393bf375c0198e5434.svg │ ├── 7d44c3b4223b041c27cb5f2749ac092b.svg │ ├── 80918a07f39f9f14a3a35a85748c7552.svg │ ├── 885839bf3fa334609583ddbd937afdfb.svg │ ├── 8ca62ba4029ee91ab7699cb6729b59d6.svg │ ├── 91f17f2d1c679df5b3e3d782966d5b8d.svg │ ├── 9294da67e8fbc8ee3f1ac635fc79c893.svg │ ├── 9b527e843dd83a6485c635f8c4366f78.svg │ ├── 9ea8a8d8d55375d9e1893a59f2241094.svg │ ├── 9f0028b414617caf75a357cfb98e7497.svg │ ├── 9f070416fd3580cf4c34c63f1690716a.svg │ ├── a8ebf8c468236800b8ed78d42ddbfa57.svg │ ├── a9a3a4a202d80326bda413b5562d5cd1.svg │ ├── aa90653a26bc63b138fb304972d81589.svg │ ├── acf5ce819219b95070be2dbeb8a671e9.svg │ ├── ad2444f8273c6541c5dc1fc5b6445401.svg │ ├── ae4fb5973f393577570881fc24fc2054.svg │ ├── af543e4935b79ac4b4967e46a594d4c2.svg │ ├── b8bc815b5e9d5177af01fd4d3d3c2f10.svg │ ├── ba499640e4485035b2f3a8d8fdd20e40.svg │ ├── be6cc5f4430c9bd99233d63a60c7448d.svg │ ├── beacfec95d202f984345b778da4cf36a.svg │ ├── c0213ff9cc036054ae01949a656bca82.svg │ ├── c0463eeb4772bfde779c20d52901d01b.svg │ ├── c06ec8be67f776ce40276258790f38a1.svg │ ├── c56e4e18ef488354b6ab1f706c305395.svg │ ├── c6c6cc9e4e541ca2e6b59518ff32294c.svg │ ├── c900aacedee846398a77ce5815fea816.svg │ ├── c9dd335ebf8f6df377ef056642179a06.svg │ ├── cdef2e61f5595278f9584a6acf7ceb0e.svg │ ├── d32d545344694514dffe290e5ab9066a.svg │ ├── d3f4e894021bcc0d88debbc6c3145196.svg │ ├── d5110898628d6270c66a13ba784e9ff1.svg │ ├── d6328eaebbcd5c358f426dbea4bdbf70.svg │ ├── db85bd6dfbbcc6817fc7960910b43296.svg │ ├── deceeaf6940a8c7a5a02373728002b0f.svg │ ├── e3950ac13bd76a194c14a76a72834d33.svg │ ├── e660f3b58b414524ec6f827411021073.svg │ ├── e88c070a4a52572ef1d5792a341c0900.svg │ ├── ea725162aae1293f076631e147b7f5cf.svg │ ├── eb4779c5fded13881cb5f169b1f10c73.svg │ ├── ecd7bf9509ecd1263b6534b1081bdb95.svg │ ├── ee06addff870fbf13ba2fc1625c447e4.svg │ ├── f063d7f0ae9a5f520626fda6c9fa41bc.svg │ ├── f0773f5b19affaedaaacc089940e33ba.svg │ ├── f4179cc6ad13e929cf7b769560131941.svg │ ├── f4868f51fc6d4d737c16f9c81af27293.svg │ ├── f93ce33e511096ed626b4719d50f17d2.svg │ └── fee38d3614300eba71fc667002031f27.svg ├── step_4_online_planning ├── README.md ├── README.tex.md ├── examples │ ├── step_4_ex_1_map_obs_to_traj_params.m │ └── step_4_ex_2_trajectory_optimization.m ├── functions │ ├── compute_turtlebot_discretized_obs.m │ ├── compute_turtlebot_point_spacings.m │ ├── compute_turtlebot_trajopt_constraints.m │ ├── turtlebot_cost.m │ ├── turtlebot_cost_for_fmincon.m │ ├── turtlebot_cost_grad.m │ ├── turtlebot_nonlcon_for_fmincon.m │ └── turtlebot_symbolic_traj_prod_model.m ├── images │ ├── step_4_ex_1_img_1.png │ ├── step_4_ex_1_img_2.png │ └── step_4_ex_2_img_1.png ├── scripts │ ├── step_4_create_turtlebot_cost_function.m │ └── step_4_run_turtlebot_simulation.m └── tex │ ├── 07e2eb627d1f673638dca7bf9b879965.svg │ ├── 21fd4e8eecd6bdf1a4d3d6bd1fb8d733.svg │ ├── 274155ee3483dbb84753ce0180293402.svg │ ├── 297f41396e8158f6a9169cfb6d6d00a5.svg │ ├── 2aa485270dee077a664c5c167a652424.svg │ ├── 2b90bd95c73be2f030b51798e34e8adb.svg │ ├── 3bf25d9d50c08c7ec96446a7abb1c024.svg │ ├── 4707fcdfb70730be9ebd54bddaa0149f.svg │ ├── 4d1719a25e4b38db1fbea7ddf5f1a886.svg │ ├── 4e0a330da614750eb556c58093edd4ae.svg │ ├── 5537cda6dac51f6d1e672a0f75f3746e.svg │ ├── 5ae1e561ec81f97d93ed9df0f76cab27.svg │ ├── 5aefecdd2b9b5de63115b522e5e53f46.svg │ ├── 5b51bd2e6f329245d425b8002d7cf942.svg │ ├── 62a1d6ae808b6d855355def103c4971f.svg │ ├── 63bb9849783d01d91403bc9a5fea12a2.svg │ ├── 6957ec2c8c11110e80efe4f039ecd7a0.svg │ ├── 6b8afc7d21c1f242a41f95fe80e39813.svg │ ├── 6bbaa8705b2be50aefae5dc1f516edb5.svg │ ├── 7087d8bef1803bb878de559f4ef83d1b.svg │ ├── 89f2e0d2d24bcf44db73aab8fc03252c.svg │ ├── 8b4f2d3e1de4d24f0d8d4b38af75c90b.svg │ ├── 9e293798cbade2bec567cf94b0aa0218.svg │ ├── a38d48f6e4e60fa5c567b920937dd86e.svg │ ├── a8809f9da005d084b0d81cc1704a2c6f.svg │ ├── a91b124f86681090d790147ababa8c23.svg │ ├── ac4347acd383a6b21d3794b0b621326e.svg │ ├── ac4bb7b235a967476d0ec663bb9588c2.svg │ ├── ad2444f8273c6541c5dc1fc5b6445401.svg │ ├── cb92288bfbb4e7352d7a04c79c0f8e99.svg │ ├── ce9da239b7dcd437e459cf571edf5e70.svg │ ├── d6328eaebbcd5c358f426dbea4bdbf70.svg │ ├── d6ebaab75d0719c06a34784e25e82aff.svg │ └── f93ce33e511096ed626b4719d50f17d2.svg ├── step_5_extras ├── README.md ├── extra_1_simulator_overview │ └── README.md ├── extra_2_writing_an_RTD_planner │ ├── README.md │ └── extra_2_run_my_turtlebot_planner_simulation.m └── extra_3_writing_an_RRT_star_planner │ ├── README.md │ ├── run_turtlebot_simulation_RRT_star_planner.m │ └── testing_RRT_HLP.m └── tex ├── 07c7f89571adef4b18e8cc11b731ddc9.svg ├── 25ed12dcaee28c65f3775ebec576007f.svg ├── 27e556cf3caa0673ac49a8f0de3c73ca.svg ├── 2db35060f2b6f146752157657cfb5d5a.svg ├── 44bc9d542a92714cac84e01cbbb7fd61.svg ├── 562282b5face354edcd2f160fd3941e0.svg ├── 6c4adbc36120d62b98deef2a20d5d303.svg ├── 7392a8cd69b275fa1798ef94c839d2e0.svg ├── 7c74eeb32158ff7c4f67d191b95450fb.svg ├── 87181ad2b235919e0785dee664166921.svg ├── 97360c281b8665f62744f3f76afff426.svg ├── ae4fb5973f393577570881fc24fc2054.svg ├── bbe77024f3f059a59d705d15a7827e6c.svg ├── c0213ff9cc036054ae01949a656bca82.svg ├── c9d7adaf70649e6eac596cb7eba2b33e.svg ├── cdcac8939f3840cd8cddf40059a4cf58.svg ├── e0393984691e829b97a04ed01dc2113e.svg └── fe5ae23c6c1a6c32f63039eddf1fe044.svg /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.DS_store* 2 | *.m~ 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, Shreyas Kousik 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /check_dependencies.m: -------------------------------------------------------------------------------- 1 | % check for simulator on path 2 | if isempty(which('simulator')) 3 | check_simulator = false ; 4 | warning(['The RTD simulator is not on your path! Please download it here: ',... 5 | 'https://github.com/skousik/simulator']) 6 | else 7 | disp('The simulator is on the MATLAB path') 8 | check_simulator = true ; 9 | end 10 | 11 | % check for RTD 12 | try 13 | is_RTD_on_path() 14 | check_RTD = true ; 15 | catch 16 | check_RTD = false ; 17 | warning(['The RTD parent repo is not on your path! Please download it here: ',... 18 | 'https://github.com/skvaskov/RTD']) 19 | end 20 | 21 | % check for spotless 22 | try 23 | x = msspoly('x') ; 24 | check_spotless = true ; 25 | catch 26 | warning(['The spotless repo is not on your path! Please download ithere: ',... 27 | 'https://github.com/spot-toolbox/spotless']) 28 | check_spotless = false ; 29 | end 30 | 31 | if check_simulator && check_RTD && check_spotless 32 | disp('RTD tutorial is ready to roll!') 33 | else 34 | warning('RTD tutorial depencies are missing!') 35 | end -------------------------------------------------------------------------------- /simulator_files/agent/turtlebot_LLC.m: -------------------------------------------------------------------------------- 1 | classdef turtlebot_LLC < low_level_controller 2 | properties 3 | % lookahead properties 4 | lookahead_time 5 | lookahead_distance 6 | lookahead_type = 'time' ; 7 | interp_mode = 'previous' ; 8 | 9 | % control gains structure 10 | gains = struct() ; 11 | default_gains = struct() ; 12 | stop_gains = struct() ; 13 | end 14 | 15 | methods 16 | %% constructor 17 | function LLC = turtlebot_LLC(varargin) 18 | n_agent_states = 4 ; 19 | n_agent_inputs = 2 ; 20 | 21 | LLC = parse_args(LLC,'n_agent_states',n_agent_states,... 22 | 'n_agent_inputs',n_agent_inputs,varargin{:}) ; 23 | end 24 | end 25 | end -------------------------------------------------------------------------------- /step_1_desired_trajectories/data/turtlebot_timing.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_1_desired_trajectories/data/turtlebot_timing.mat -------------------------------------------------------------------------------- /step_1_desired_trajectories/examples/step_1_ex_1_desired_trajectory.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % 3 | % Create and plot an example desired trajectory. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 15 May 2019 7 | % Updated: 7 June 2019 8 | % 9 | %% user parameters 10 | t_f = 1 ; 11 | w_des = 0.5 ; % rad/s 12 | v_des = 1.0 ; % m/s 13 | 14 | %% automated from here 15 | [T,U,Z] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) ; 16 | 17 | % get the x and y positions of the trajectory 18 | x = Z(1,:) ; 19 | y = Z(2,:) ; 20 | 21 | % plot 22 | figure(1) ; 23 | plot(x,y,'b--','LineWidth',1.5) 24 | axis equal 25 | legend('desired traj.') 26 | set(gca,'FontSize',15) -------------------------------------------------------------------------------- /step_1_desired_trajectories/examples/step_1_ex_2_trajectory_tracking.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % 3 | % This script demonstrates the turtlebot's trajectory-tracking behavior 4 | % using the default PD controller (see A.LLC). 5 | % 6 | % Author: Shreyas Kousik 7 | % Created: 12 May 2019 8 | % Updated: 24 Oct 2019 9 | % 10 | %% run the trajectory tracking 11 | A = turtlebot_agent() ; 12 | v0 = 0.5 ; 13 | A.reset([0;0;0;0.5]) 14 | 15 | t_f = 1 ; 16 | w_des = 0.5 ; % rad/s 17 | v_des = 1.0 ; % m/s 18 | [T,U,Z] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) ; 19 | 20 | t_total = 1 ; 21 | A.move(t_total,T,U,Z) 22 | 23 | %% plotting 24 | figure(1) ; clf ; axis equal ; hold on ; set(gca,'FontSize',15) 25 | 26 | plot(Z(1,:),Z(2,:),'b--','LineWidth',1.5) 27 | plot(A) 28 | A.animate() 29 | -------------------------------------------------------------------------------- /step_1_desired_trajectories/examples/step_1_ex_3_braking_trajectory.m: -------------------------------------------------------------------------------- 1 | % description 2 | % 3 | % Make an example desired trajectory for the TurtleBot, and show the robot 4 | % trying to track it, then brake along it. 5 | % 6 | % Author: Shreyas Kousik 7 | % Created: 15 May 2019 8 | % Updated: 24 Oct 2019 9 | % 10 | %% user parameters 11 | % desired trajectory 12 | w_des = 1 ; % rad/s 13 | v_des = 1 ; % m/s 14 | 15 | % timing 16 | t_plan = 0.5 ; % s 17 | 18 | % agent initial state 19 | v_0 = 1.25 ; % rad/s 20 | 21 | %% automated from here 22 | % set up agent 23 | A = turtlebot_agent() ; 24 | 25 | % set up trajectory 26 | t_stop = v_des/A.max_accel ; 27 | [T_brk,U_brk,Z_brk] = make_turtlebot_braking_trajectory(t_plan,t_stop,w_des,v_des) ; 28 | 29 | %% track the braking trajectory 30 | % reset the agent to its initial speed 31 | z0 = [0;0;0;v_0] ; 32 | A.reset(z0) ; 33 | 34 | % execute desired trajectory with braking 35 | A.move(T_brk(end),T_brk,U_brk,Z_brk) 36 | 37 | %% plot agent 38 | close all 39 | 40 | figure(1) ; hold on ;axis equal 41 | 42 | plot_path(A.state(1:2,:),'LineWidth',1.5) 43 | plot_path(Z_brk(1:2,:),'b--','LineWidth',1.5) 44 | plot(A) 45 | 46 | % set axes 47 | xlo = min(A.state(1,:)) - 0.25 ; 48 | xhi = max(A.state(1,:)) + 0.25 ; 49 | ylo = min(A.state(2,:)) - 0.25 ; 50 | yhi = max(A.state(2,:)) + 0.25 ; 51 | axis([xlo,xhi,ylo,yhi]) 52 | 53 | % label plot 54 | xlabel('x [m]') 55 | ylabel('y [m]') 56 | legend('executed traj.','desired traj.') 57 | set(gca,'FontSize',15) 58 | 59 | % animate agent 60 | A.animate 61 | 62 | %% plot desired trajectories 63 | figure(2) ; clf ; 64 | plot(T_brk,Z_brk','LineWidth',1.5) 65 | xlabel('Time [s]') 66 | ylabel('States') 67 | title('Desired Trajectory') 68 | legend('x','y','\theta','v') 69 | set(gca,'FontSize',15) -------------------------------------------------------------------------------- /step_1_desired_trajectories/functions/get_k_from_w_and_v.m: -------------------------------------------------------------------------------- 1 | function k = get_k_from_w_and_v(FRS,w,v) 2 | % k = get_k_from_w_and_v(FRS,w,v) 3 | % 4 | % Given an FRS struct output from computing the Turtlebot FRS, and values 5 | % of angular and linear velocity w and v, return the corresponding k \in K 6 | % 7 | % Author: Shreyas Kousik 8 | % Created: 26 Oct 2019 9 | % Updated: 28 Oct 2019 10 | 11 | % allocate 12 | k = nan(2,1) ; 13 | 14 | % get k_1 15 | k(1) = w / FRS.w_max ; 16 | 17 | % get k_2 18 | k(2) = (2/diff(FRS.v_range)).*(v - mean(FRS.v_range)) ; 19 | end -------------------------------------------------------------------------------- /step_1_desired_trajectories/functions/get_t_f_from_v_0.m: -------------------------------------------------------------------------------- 1 | function t_f = get_t_f_from_v_0(v_0,t_f_all,v_0_min,v_0_max) 2 | % t_f = get_t_f_from_v_0(v_0,t_f_all,v_0_min,v_0_max) 3 | % 4 | % Given an initial speed v_0, return the time horizon t_f required for the 5 | % FRS computation corresponding to the highest speed range that v_0 is in. 6 | % 7 | % Author: Shreyas Kousik 8 | % Created: 25 Oct 2019 9 | % Updated: 26 Oct 2019 10 | 11 | if nargin < 2 12 | t_f_all = [0.65 0.80 0.90] ; 13 | end 14 | 15 | if nargin < 3 16 | v_0_min = [0.0 0.5 1.0] ; 17 | end 18 | 19 | if nargin < 4 20 | v_0_max = [0.5 1.0 1.5] ; 21 | end 22 | 23 | % figure out which interval the given v_0 is in 24 | idxs = (v_0 >= v_0_min) & (v_0 <= v_0_max) ; 25 | 26 | % get t_f 27 | t_f = max(t_f_all(idxs)) ; 28 | end -------------------------------------------------------------------------------- /step_1_desired_trajectories/functions/get_t_stop_from_v.m: -------------------------------------------------------------------------------- 1 | function t_stop = get_t_stop_from_v(v,accel) 2 | % t_stop = get_t_stop_from_v(v,accel) 3 | % 4 | % Given a current speed v, compute the amount of time needed for the 5 | % Turtlebot to brake to a stop, given a maximum allowable acceleration 6 | % (accel). 7 | % 8 | % Author: Shreyas Kousik 9 | % Created: 24 Oct 2019 10 | % Updated: 27 Oct 2019 11 | 12 | if nargin < 2 13 | accel = 2 ; 14 | end 15 | 16 | t_stop = v ./ accel ; 17 | end -------------------------------------------------------------------------------- /step_1_desired_trajectories/functions/get_turtlebot_braking_scale_at_time.m: -------------------------------------------------------------------------------- 1 | function s = get_turtlebot_braking_scale_at_time(t_cur,t_plan,t_stop,pwr) 2 | % s = get_turtlebot_braking_scale_at_time(t_cur,t_plan,t_stop,pwr) 3 | % 4 | % Return the scalar value s \in [0,1] by which to scale a trajectory's 5 | % desired angular and linear speeds to cause the turtlebot to brake to a 6 | % stop over the time interval [t_plan, t_plan + t_stop]. 7 | % 8 | % Author: Shreyas Kousik 9 | % Created: 23 Oct 2019 10 | % Updated: - 11 | 12 | if nargin < 2 13 | t_plan = 0.5 ; 14 | t_stop = 2.61 ; 15 | pwr = 4 ; 16 | end 17 | 18 | s = ((t_stop - t_cur + t_plan)./(t_stop)).^pwr ; 19 | end -------------------------------------------------------------------------------- /step_1_desired_trajectories/functions/make_turtlebot_braking_trajectory.m: -------------------------------------------------------------------------------- 1 | function [T,U,Z] = make_turtlebot_braking_trajectory(t_plan,t_stop,w_des,v_des) 2 | % [T,U,Z] = make_turtlebot_braking_trajectory(t_plan,t_stop,w_des,v_des) 3 | % 4 | % Create a Dubins path with braking as a full-state trajectory for the 5 | % TurtleBot. 6 | % 7 | % The inputs are: 8 | % t_plan planning timeout 9 | % t_f planning time horizon 10 | % t_stop duration required for robot to come to a stop 11 | % w_des desired yaw rate 12 | % v_des desired speed 13 | % 14 | % The outputs are: 15 | % T timing for desired trajectory as a 1-by-N array 16 | % U desired input (yaw rate and acceleration) as 2-by-N array 17 | % Z desired trajectory (x,y,h,v) as a 4-by-N array 18 | % 19 | % Author: Shreyas Kousik 20 | % Created: 12 May 2019 21 | % Updated: 24 Oct 2019 22 | 23 | % set up timing 24 | t_sample = 0.01 ; 25 | t_total = t_plan + t_stop ; 26 | T = unique([0:t_sample:t_total,t_total]); 27 | 28 | % create braking traj. for w and v 29 | t_log = T >= t_plan ; 30 | braking_scale_power = 1 ; 31 | scale = ones(size(T)) ; 32 | scale(t_log) = get_turtlebot_braking_scale_at_time(T(t_log),t_plan,t_stop,braking_scale_power) ; 33 | 34 | % get inputs for desired trajectories 35 | w_traj = w_des.*scale ; 36 | v_traj = v_des.*scale ; 37 | U_in = [w_traj ; v_traj] ; 38 | 39 | % compute desired trajectory 40 | z0 = zeros(3,1) ; 41 | [~,Z] = ode45(@(t,z) turtlebot_trajectory_producing_model(t,z,T,U_in),T,z0) ; 42 | 43 | % append velocity to (x,y,h) trajectory to make it a full-state 44 | % trajectory for the turtlebot 45 | Z = [Z' ; v_traj] ; 46 | 47 | % compute inputs for robot 48 | a_traj = diff(v_traj)./t_sample ; 49 | U = [w_traj ; [a_traj, a_traj(end)]] ; 50 | end -------------------------------------------------------------------------------- /step_1_desired_trajectories/functions/make_turtlebot_desired_trajectory.m: -------------------------------------------------------------------------------- 1 | function [T,U,Z] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) 2 | % [T,U,Z] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) 3 | % 4 | % Create a Dubins path as a full-state trajectory for the TurtleBot. 5 | % 6 | % The inputs are: 7 | % t_f planning time horizon 8 | % w_des desired yaw rate 9 | % v_des desired speed 10 | % 11 | % The outputs are: 12 | % T timing for desired trajectory as a 1-by-N array 13 | % U desired input (yaw rate and acceleration) as 2-by-N array 14 | % Z desired trajectory (x,y,h,v) as a 4-by-N array 15 | % 16 | % Author: Shreyas Kousik 17 | % Created: 12 May 2019 18 | % Updated: 28 May 2019 19 | 20 | % set up timing 21 | t_sample = 0.01 ; 22 | T = unique([0:t_sample:t_f,t_f],'stable'); 23 | N_t = length(T) ; 24 | 25 | % get inputs for desired trajectories 26 | w_traj = w_des*ones(1,N_t) ; 27 | v_traj = v_des*ones(1,N_t) ; 28 | U_in = [w_traj ; v_traj] ; 29 | 30 | % compute desired trajectory 31 | z0 = zeros(3,1) ; 32 | [~,Z] = ode45(@(t,z) turtlebot_trajectory_producing_model(t,z,T,U_in),T,z0) ; 33 | 34 | % append velocity to (x,y,h) trajectory to make it a full-state 35 | % trajectory for the turtlebot 36 | Z = [Z' ; v_traj] ; 37 | 38 | % compute inputs for robot 39 | a_traj = [diff(v_traj)./t_sample, 0] ; 40 | U = [w_traj ; a_traj] ; 41 | end -------------------------------------------------------------------------------- /step_1_desired_trajectories/functions/turtlebot_trajectory_producing_model.m: -------------------------------------------------------------------------------- 1 | function zd = turtlebot_trajectory_producing_model(t,z,T_in,U_in) 2 | % zd = turtlebot_trajectory_producing_model(t,z,T_in,U_in) 3 | % 4 | % Output the dynamics of the simplified Dubins' car that we use as a 5 | % trajectory-producing model for the TurtleBot. This can get plugged in to 6 | % ode45, for example. See make_turtlebot_desired_trajectory for how it is 7 | % used. 8 | % 9 | % Author: Shreyas Kousik 10 | % Created: 12 May 2019 11 | % Updated: 28 Oct 2019 12 | 13 | % get heading state 14 | h = z(3) ; 15 | 16 | % get inputs 17 | u = match_trajectories(t,T_in,U_in) ; 18 | w_des = u(1) ; 19 | v_des = u(2) ; 20 | 21 | % compute dynamics 22 | zd = [v_des*cos(h) ; 23 | v_des*sin(h) ; 24 | w_des ] ; 25 | end -------------------------------------------------------------------------------- /step_1_desired_trajectories/images/step_1.5_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_1_desired_trajectories/images/step_1.5_img_1.png -------------------------------------------------------------------------------- /step_1_desired_trajectories/images/step_1.5_img_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_1_desired_trajectories/images/step_1.5_img_2.png -------------------------------------------------------------------------------- /step_1_desired_trajectories/images/step_1_ex_1_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_1_desired_trajectories/images/step_1_ex_1_img_1.png -------------------------------------------------------------------------------- /step_1_desired_trajectories/images/step_1_ex_2_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_1_desired_trajectories/images/step_1_ex_2_img_1.png -------------------------------------------------------------------------------- /step_1_desired_trajectories/images/step_1_ex_3_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_1_desired_trajectories/images/step_1_ex_3_img_1.png -------------------------------------------------------------------------------- /step_1_desired_trajectories/scripts/step_1_inspect_braking_distance_vs_initial_speed.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script considers how the Turtlebot's braking distance increases as a 3 | % function of its initial speed, and uses that information to determine the 4 | % time horizon required for reachability analysis. 5 | % 6 | % We also consider that, at each receding-horizon planning iteration, we 7 | % are able to choose a commanded speed of our current speed plus or minus 8 | % some change, delta_v: 9 | % 10 | % v_cmd = v_0 +/- delta_v 11 | % 12 | % This means that we could be braking from a higher speed than our initial 13 | % speed at the beginning of the planning iteration. Therefore, we need to 14 | % increase our braking time estimate to compensate. 15 | % 16 | % Author: Shreyas Kousik 17 | % Created: 24 Oct 2019 18 | % Updated: 25 Oct 2019 19 | % 20 | %% user parameters 21 | % speed range to consider 22 | v_min = 0.1 ; % needs to be > 0 for the braking traj to make sense 23 | v_max = 1.5 ; 24 | 25 | % get the allowed commandable change in the turtlebot's speed 26 | delta_v = 0.25 ; % m/s 27 | 28 | %% automated from here 29 | % create vector of initial speeds 30 | v_0_vec = v_min:0.1:v_max ; 31 | 32 | % set up turtlebot agent 33 | A = turtlebot_agent() ; 34 | 35 | % get vector for distances traveled 36 | d_brk = nan(size(v_0_vec)) ; 37 | 38 | % for each initial speed, generate a braking trajectory and then execute it 39 | t_plan = 0.5 ; 40 | idx = 1 ; 41 | for v_0 = v_0_vec 42 | % get stopping time for current t_stop 43 | t_stop = v_0/A.max_accel ; 44 | 45 | % set up braking trajectoryt 46 | w_des = 0 ; 47 | v_des = v_0 ; 48 | [T_brk,U_brk,Z_brk] = make_turtlebot_braking_trajectory(t_plan,t_stop,w_des,v_des) ; 49 | 50 | % track braking trajectory 51 | z0 = [0;0;0;v_0] ; 52 | A.reset(z0) 53 | A.move(T_brk(end),T_brk,U_brk,Z_brk) ; 54 | 55 | % find the total distance traveled during braking 56 | T = A.time ; 57 | d_at_t_plan = match_trajectories(t_plan,A.time,A.state(1,:)) ; 58 | d_brk(idx) = A.state(1,end) - d_at_t_plan ; 59 | idx = idx + 1 ; 60 | end 61 | 62 | %% compute t_f for each of the max initial speed ranges 63 | % get t_stop for all the braking distances 64 | t_stop_vec = d_brk ./ v_0 ; 65 | 66 | % round up to the nearest 0.1 s to preempt numerical errors 67 | t_stop_vec = ceil(10*t_stop_vec)./10 ; 68 | 69 | % get t_f for the three FRSes we plan to compute later 70 | v_0_all = [0.5 1.0 1.5] ; 71 | v_begin_braking = v_0_all + delta_v.*[1 1 0] ; 72 | t_f_all = match_trajectories(v_begin_braking,v_0_vec,t_stop_vec) + t_plan ; 73 | 74 | %% save timing 75 | save('turtlebot_timing.mat','t_plan','t_f_all',... 76 | 'v_0_all','v_begin_braking','delta_v') ; 77 | 78 | %% plot v_0 vs d 79 | figure(1) ; clf ; hold on ; grid on ; 80 | plot(v_0_vec,d_brk,'LineWidth',1.5) 81 | xlabel('v_0 [m/s]') 82 | ylabel('braking distance [m]') 83 | set(gca,'FontSize',15) -------------------------------------------------------------------------------- /step_1_desired_trajectories/scripts/step_1_validate_t_f.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script checks that a non-braking trajectory will travel farther than 3 | % a braking trajectory, given the appropriate duration. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 24 Oct 2019 7 | % Updated: - 8 | % 9 | %% automated from here 10 | % set up speed and yaw rate 11 | v_max = 1.5 ; % m/s 12 | w_des = 0.0 ; % rad/s 13 | 14 | % set up timing 15 | t_plan = 0.5; % s 16 | 17 | % create turtlebot 18 | A = turtlebot_agent() ; 19 | 20 | % create non-braking trajectory 21 | t_f = t_plan + 0.4 ; 22 | [T_go,~,Z_go] = make_turtlebot_desired_trajectory(t_f,w_des,v_max) ; 23 | 24 | % create braking trajectory 25 | t_stop = v_max / A.max_accel ; 26 | [T_brk,~,Z_brk] = make_turtlebot_braking_trajectory(t_plan,t_stop,w_des,v_max) ; 27 | 28 | %% plot 29 | figure(1) ; clf ; hold on ; grid on ; 30 | 31 | % plot trajectories 32 | plot(T_go,Z_go(1,:),T_brk,Z_brk(1,:),'LineWidth',1.5) 33 | plot([0,1.5],max(Z_brk(1,:)).*ones(1,2),'k--') 34 | 35 | % label plot 36 | xlabel('time [s]') 37 | ylabel('distance [m]') 38 | legend('non-braking','braking','Location','SouthEast') 39 | set(gca,'FontSize',15) -------------------------------------------------------------------------------- /step_1_desired_trajectories/tex/27e556cf3caa0673ac49a8f0de3c73ca.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_1_desired_trajectories/tex/2db35060f2b6f146752157657cfb5d5a.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /step_1_desired_trajectories/tex/332cc365a4987aacce0ead01b8bdcc0b.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_1_desired_trajectories/tex/6c4adbc36120d62b98deef2a20d5d303.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_1_desired_trajectories/tex/ae4fb5973f393577570881fc24fc2054.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_1_desired_trajectories/tex/deceeaf6940a8c7a5a02373728002b0f.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_1_desired_trajectories/tex/e18b24c87a7c52fd294215d16b42a437.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_2_error_function/data/turtlebot_error_functions_v_0_0.0_to_0.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_2_error_function/data/turtlebot_error_functions_v_0_0.0_to_0.5.mat -------------------------------------------------------------------------------- /step_2_error_function/data/turtlebot_error_functions_v_0_0.5_to_1.0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_2_error_function/data/turtlebot_error_functions_v_0_0.5_to_1.0.mat -------------------------------------------------------------------------------- /step_2_error_function/data/turtlebot_error_functions_v_0_1.0_to_1.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_2_error_function/data/turtlebot_error_functions_v_0_1.0_to_1.5.mat -------------------------------------------------------------------------------- /step_2_error_function/examples/step_2_ex_1_tracking_error_single_traj.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script demonstrates the tracking error computation for a single 3 | % trajectory of the TurtleBot. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 16 May 2019 7 | % Updated: 28 Oct 2019 8 | % 9 | %% user parameters 10 | % initial condition (we only care about the initial condition in speed, 11 | % because the dynamics are position/rotation invariant) 12 | v_0 = 0.75 ; % m/s 13 | 14 | % command bounds 15 | w_des = 1.0 ; % rad/s ; 16 | v_des = 1.0 ; % m/s 17 | 18 | %% automated from here 19 | % create turtlebot 20 | A = turtlebot_agent ; 21 | 22 | % create the initial condition 23 | z0 = [0;0;0;v_0] ; % (x,y,h,v) 24 | A.reset(z0) 25 | 26 | %% trajectory setup and tracking 27 | % make the desired trajectory 28 | t_f = get_t_f_from_v_0(v_0) ; 29 | [T_des,U_des,Z_des] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) ; 30 | 31 | % track the desired trajectory 32 | A.move(T_des(end),T_des,U_des,Z_des) ; 33 | 34 | %% tracking error computation 35 | % get the realized position trajectory 36 | T = A.time ; 37 | Z = A.state(A.position_indices,:) ; 38 | 39 | % interpolate the realized trajectory to match the braking traj timing 40 | pos = match_trajectories(T_des,T,Z) ; 41 | 42 | % get the desired trajectory 43 | pos_des = Z_des(1:2,:) ; 44 | 45 | % compute the tracking error 46 | pos_err = abs(pos - pos_des) ; 47 | x_err = pos_err(1,:) ; 48 | y_err = pos_err(2,:) ; 49 | 50 | %% plot robot 51 | figure(1) ; clf ; hold on ; axis equal ; grid on ; 52 | plot_path(Z_des,'b--','LineWidth',1.5) 53 | plot_path(Z,'b','LineWidth',1.5) 54 | plot(A) 55 | legend('desired traj','realized traj') 56 | title('robot showing tracking error') 57 | xlabel('x [m]') 58 | ylabel('y [m]') 59 | set(gca,'FontSize',15) 60 | 61 | %% plot tracking error 62 | figure(2) ; clf ; hold on ; 63 | plot(T_des,x_err,'--','Color',[0.5 0.2 0.2],'LineWidth',1.5) 64 | plot(T_des,y_err,'--','Color',[0.2 0.5 0.2],'LineWidth',1.5) 65 | title('tracking error in x and y') 66 | xlabel('time [s]') 67 | ylabel('error [m]') 68 | legend('x error','y error') 69 | set(gca,'FontSize',15) -------------------------------------------------------------------------------- /step_2_error_function/images/step_2.2_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_2_error_function/images/step_2.2_img_1.png -------------------------------------------------------------------------------- /step_2_error_function/images/step_2_ex_1_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_2_error_function/images/step_2_ex_1_img_1.png -------------------------------------------------------------------------------- /step_2_error_function/images/step_2_ex_1_img_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_2_error_function/images/step_2_ex_1_img_2.png -------------------------------------------------------------------------------- /step_2_error_function/images/step_2_ex_2_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_2_error_function/images/step_2_ex_2_img_1.png -------------------------------------------------------------------------------- /step_2_error_function/scripts/step_2_inspect_turtlebot_desired_vs_braking_traj.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script is to check that, for any desired trajectory and initial 3 | % condition, the associated braking trajectory travels less far. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 28 Oct 2019 7 | % Updated: - 8 | % 9 | %% user parameters 10 | % desired trajectory parameters 11 | w_des = 1 ; 12 | v_des = 1 ; 13 | 14 | % initial condition 15 | v_0 = 1.25 ; 16 | 17 | %% automated from here 18 | % make bot 19 | A = turtlebot_agent() ; 20 | 21 | % get timing 22 | t_plan = 0.5 ; 23 | t_f = get_t_f_from_v_0(v_0) ; 24 | t_stop = v_des / A.max_accel ; 25 | 26 | % get desired traj 27 | [T_go,U_go,Z_go] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) ; 28 | 29 | % get braking traj 30 | [T_brk,U_brk,Z_brk] = make_turtlebot_braking_trajectory(t_plan,t_stop,w_des,v_des) ; 31 | 32 | % track braking traj 33 | A.reset([0;0;0;v_0]) ; 34 | % A.move(T_brk(end),T_brk,U_brk,Z_brk) ; 35 | A.move(T_go(end),T_go,U_go,Z_go) ; 36 | 37 | %% plotting 38 | figure(1) ; clf ; hold on ; axis equal ; 39 | 40 | plot(A) 41 | h1 = plot_path(Z_go,'b--','LineWidth',1.5) ; 42 | h2 = plot_path(Z_brk,'r--','LineWidth',1.5) ; 43 | legend([h1 h2],'desired traj','braking traj') 44 | xlabel('x [m]') 45 | ylabel('y [m]') 46 | set(gca,'FontSize',15) -------------------------------------------------------------------------------- /step_2_error_function/tex/27e556cf3caa0673ac49a8f0de3c73ca.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_2_error_function/tex/332cc365a4987aacce0ead01b8bdcc0b.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_2_error_function/tex/3cf4fbd05970446973fc3d9fa3fe3c41.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_2_error_function/tex/6bac6ec50c01592407695ef84f457232.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_2_error_function/tex/6c4adbc36120d62b98deef2a20d5d303.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_2_error_function/tex/751613a1a4da78db7647a339cbf261c3.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /step_2_error_function/tex/ae4fb5973f393577570881fc24fc2054.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_2_error_function/tex/deceeaf6940a8c7a5a02373728002b0f.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_10_v_0_0.0_to_0.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_10_v_0_0.0_to_0.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_10_v_0_0.5_to_1.0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_10_v_0_0.5_to_1.0.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_10_v_0_1.0_to_1.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_10_v_0_1.0_to_1.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_12_v_0_0.0_to_0.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_12_v_0_0.0_to_0.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_12_v_0_0.5_to_1.0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_12_v_0_0.5_to_1.0.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_12_v_0_1.0_to_1.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_12_v_0_1.0_to_1.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_4_v_0_0.0_to_0.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_4_v_0_0.0_to_0.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_4_v_0_0.5_to_1.0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_4_v_0_0.5_to_1.0.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_4_v_0_1.0_to_1.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_4_v_0_1.0_to_1.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_6_v_0_0.0_to_0.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_6_v_0_0.0_to_0.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_6_v_0_0.5_to_1.0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_6_v_0_0.5_to_1.0.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_6_v_0_1.0_to_1.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_6_v_0_1.0_to_1.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_8_v_0_0.0_to_0.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_8_v_0_0.0_to_0.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_8_v_0_0.5_to_1.0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_8_v_0_0.5_to_1.0.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_8_v_0_1.0_to_1.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/reach_sets/turtlebot_FRS_deg_8_v_0_1.0_to_1.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/scaling/turtlebot_FRS_scaling_v_0_0.0_to_0.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/scaling/turtlebot_FRS_scaling_v_0_0.0_to_0.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/scaling/turtlebot_FRS_scaling_v_0_0.5_to_1.0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/scaling/turtlebot_FRS_scaling_v_0_0.5_to_1.0.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/data/scaling/turtlebot_FRS_scaling_v_0_1.0_to_1.5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/data/scaling/turtlebot_FRS_scaling_v_0_1.0_to_1.5.mat -------------------------------------------------------------------------------- /step_3_FRS_computation/examples/step_3_ex_2_visualize_turtlebot_FRS.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script plots the output of a saved Turtlebot FRS in the FRS frame 3 | % and the global frame 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 28 Oct 2019 7 | % Updated: 29 Oct 2019 8 | % 9 | clear ; clc ; close all 10 | %% user parameters 11 | % initial speed 12 | v_0 = 1.25 ; % m/s 13 | 14 | % yaw rate and speed 15 | w_des = 0.4 ; % rad/s 16 | v_des = 1.25 ; % m/s 17 | 18 | % degree FRS to visualize (pick 4, 6, 8, 10, or 12) 19 | degree = 6 ; 20 | 21 | %% automated from here 22 | % load timing 23 | load('turtlebot_timing.mat') 24 | 25 | % load the right FRS 26 | FRS = get_FRS_from_v_0(v_0,degree) ; 27 | 28 | % fix the initial speed 29 | if abs(v_des - v_0) > delta_v 30 | error('Please pick v_0 and v_des so that |v_0 - v_des| <= delta_v') 31 | end 32 | 33 | %% set up and move the turtlebot 34 | % get agent 35 | A = turtlebot_agent ; 36 | 37 | % create the initial condition 38 | z0 = [0;0;0;v_0] ; % (x,y,h,v) 39 | 40 | % create the desired trajectory 41 | t_f = FRS.time_scale ; 42 | [T_des,U_des,Z_des] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) ; 43 | 44 | % put trajectory into FRS frame 45 | Z_FRS = world_to_FRS(Z_des(1:2,:),zeros(3,1),FRS.initial_x,FRS.initial_y,FRS.distance_scale) ; 46 | 47 | % create the braking trajectory for the robot to track in the world frame 48 | t_stop = get_t_stop_from_v(v_des) ; 49 | [T_brk,U_brk,Z_brk] = make_turtlebot_braking_trajectory(t_plan,t_stop,w_des,v_des) ; 50 | 51 | % move the robot 52 | A.reset(z0) 53 | A.move(T_brk(end),T_brk,U_brk,Z_brk) ; 54 | 55 | %% prep to plot FRS 56 | % create k_eval from w_des and v_des 57 | k_eval = get_k_from_w_and_v(FRS,w_des,v_des) ; 58 | 59 | % get FRS initial condition set and variables 60 | z = FRS.z ; 61 | k = FRS.k ; 62 | h_Z0 = FRS.h_Z0 ; 63 | 64 | % get FRS polynomial 65 | I = FRS.FRS_polynomial ; 66 | I_z = msubs(I,FRS.k,k_eval) ; 67 | 68 | %% plot in FRS frame 69 | figure(1) ; 70 | subplot(1,2,1) ; hold on ; axis equal ; 71 | 72 | % plot initial condition 73 | plot_2D_msspoly_contour(h_Z0,z,0,'LineWidth',1.5,'Color','b') 74 | 75 | % plot FRS subset for given k 76 | plot_2D_msspoly_contour(I_z,z,1,'LineWidth',1.5,'Color',[0.1 0.8 0.3]) 77 | 78 | % plot desired trajectory 79 | plot_path(Z_FRS,'b--','LineWidth',1.5) 80 | 81 | set(gca,'FontSize',15) 82 | axis([-1,1,-1,1]) 83 | xlabel('x / (dist. scale)') 84 | ylabel('y / (dist. scale)') 85 | title('FRS Frame') 86 | 87 | %% plot in global frame 88 | subplot(1,2,2) ; hold on ; axis equal; 89 | 90 | % plot the initial condition 91 | offset = -FRS.distance_scale*[FRS.initial_x ; FRS.initial_y] ; 92 | plot_2D_msspoly_contour(h_Z0,z,0,'LineWidth',1.5,'Color','b',... 93 | 'Offset',offset,'Scale',FRS.distance_scale) 94 | 95 | % plot the FRS 96 | plot_2D_msspoly_contour(I_z,z,1,'LineWidth',1.5,'Color',[0.1 0.8 0.3],... 97 | 'Offset',offset,'Scale',FRS.distance_scale) 98 | 99 | % plot the desired trajectory 100 | plot_path(Z_des,'b--','LineWidth',1.5) 101 | 102 | % plot the agent 103 | plot(A) 104 | 105 | % labeling 106 | set(gca,'FontSize',15) 107 | xlabel('x [m]') 108 | ylabel('y [m]') 109 | title('World Frame') -------------------------------------------------------------------------------- /step_3_FRS_computation/examples/step_3_ex_3_inspect_FRS_conservatism.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script visualizes the TurtleBot FRS computed for degrees 6, 8, and 3 | % 10 on one plot. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 21 May 2019 7 | % Updated: 26 Oct 2019 8 | % 9 | %% user parameters 10 | % trajectory to evaluate 11 | w_des = 1 ; 12 | v_des = 1.5 ; 13 | 14 | % robot initial condition 15 | v_0 = 1.25 ; 16 | 17 | %% automated from here 18 | % load timing 19 | load('turtlebot_timing.mat') 20 | 21 | % load the FRSes 22 | FRS_06 = get_FRS_from_v_0(v_0,6) ; 23 | FRS_08 = get_FRS_from_v_0(v_0,8) ; 24 | FRS_10 = get_FRS_from_v_0(v_0,10) ; 25 | 26 | % get one h_Z0 to plot 27 | h_Z0 = FRS_06.h_Z0 ; 28 | z = FRS_06.z ; 29 | k = FRS_06.k ; 30 | 31 | %% create agent for visualization 32 | % get agent 33 | A = turtlebot_agent ; 34 | 35 | % create k_eval from w_des and v_des 36 | k_eval = get_k_from_w_and_v(FRS_06,w_des,v_des) ; 37 | 38 | % fix the initial speed 39 | if abs(v_des - v_0) > delta_v 40 | error('Please pick v_0 and v_des so that |v_0 - v_des| <= delta_v') 41 | end 42 | 43 | % create the initial condition 44 | z0 = [0;0;0;v_0] ; % (x,y,h,v) 45 | 46 | % create the desired trajectory 47 | t_f = FRS_06.time_scale ; 48 | [T_des,U_des,Z_des] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) ; 49 | 50 | % put trajectory into FRS frame 51 | Z_FRS = world_to_FRS(Z_des(1:2,:),zeros(3,1),FRS_06.initial_x,FRS_06.initial_y,FRS_06.distance_scale) ; 52 | 53 | % move the robot 54 | A.reset(z0) 55 | A.move(T_des(end),T_des,U_des,Z_des) ; 56 | 57 | 58 | %% create offsets and distance scales for the FRSes 59 | D4 = FRS_06.distance_scale ; 60 | O4 = -D4 * [FRS_06.initial_x ; FRS_06.initial_y] ; 61 | 62 | D6 = FRS_08.distance_scale ; 63 | O6 = -D6 * [FRS_08.initial_x ; FRS_08.initial_y] ; 64 | 65 | D8 = FRS_10.distance_scale ; 66 | O8= -D8 * [FRS_10.initial_x ; FRS_10.initial_y] ; 67 | 68 | %% plot 69 | figure(1) ; clf ; hold on ; axis equal 70 | 71 | % plot the initial condition 72 | plot_2D_msspoly_contour(h_Z0,z,0,'LineWidth',1.5,'Color','b',... 73 | 'Offset',O4,'Scale',D4) 74 | 75 | % plot degree 4 FRS 76 | I_z_4 = msubs(FRS_06.FRS_polynomial,k,k_eval) ; 77 | plot_2D_msspoly_contour(I_z_4,z,1,'LineWidth',1.5,'Color',[0.1 0.8 0.3],... 78 | 'Offset',O4,'Scale',D4) 79 | 80 | % plot degree 6 FRS 81 | I_z_6 = msubs(FRS_08.FRS_polynomial,k,k_eval) ; 82 | plot_2D_msspoly_contour(I_z_6,z,1,'LineWidth',1.5,'Color',0.6*[0.1 1 0.3],... 83 | 'Offset',O6,'Scale',D6) 84 | 85 | % plot degree 8 FRS 86 | I_z_8 = msubs(FRS_10.FRS_polynomial,k,k_eval) ; 87 | plot_2D_msspoly_contour(I_z_8,z,1,'LineWidth',1.5,'Color',0.4*[0.1 1 0.3],... 88 | 'Offset',O8,'Scale',D8) 89 | 90 | % plot the desired (braking) trajectory 91 | plot_path(Z_des,'b--','LineWidth',1.5) 92 | 93 | % plot the agent 94 | plot(A) 95 | 96 | % labeling 97 | xlabel('x [m]') 98 | ylabel('y [m]') 99 | set(gca,'FontSize',15) -------------------------------------------------------------------------------- /step_3_FRS_computation/functions/get_FRS_from_v_0.m: -------------------------------------------------------------------------------- 1 | function FRS = get_FRS_from_v_0(v_0,degree) 2 | % FRS = get_FRS_from_v_0(v_0) 3 | % 4 | % Load the fastest feasible FRS for the given initial speed v_0 5 | % 6 | % Author: Shreyas Kousik 7 | % Created: 28 Oct 2019 8 | % Updated: - 9 | 10 | if nargin < 2 11 | degree = 10 ; 12 | end 13 | 14 | v_0_range = get_v_0_range_from_v_0(v_0) ; 15 | 16 | switch v_0_range(1) 17 | case 0.0 18 | FRS = load(['turtlebot_FRS_deg_',num2str(degree),'_v_0_0.0_to_0.5.mat']) ; 19 | case 0.5 20 | FRS = load(['turtlebot_FRS_deg_',num2str(degree),'_v_0_0.5_to_1.0.mat']) ; 21 | case 1.0 22 | FRS = load(['turtlebot_FRS_deg_',num2str(degree),'_v_0_1.0_to_1.5.mat']) ; 23 | otherwise 24 | error('Hey! You picked an invalid speed range for the tutorial!') 25 | end 26 | end -------------------------------------------------------------------------------- /step_3_FRS_computation/functions/get_v_0_range_from_v_0.m: -------------------------------------------------------------------------------- 1 | function v_0_range = get_v_0_range_from_v_0(v_0,v_0_min,v_0_max) 2 | % v_0_range = get_v_0_range_from_v_0(v_0) 3 | % v_0_range = get_v_0_range_from_v_0(v_0,v_0_min,v_0_max) 4 | % 5 | % Given a value of v_0, return the v_0 range that contains v_0 (and 6 | % corresponds to a particular FRS for the turtlebot) 7 | % 8 | % Author: Shreyas Kousik 9 | % Created: 26 Oct 2019 10 | % Updated: - 11 | 12 | if nargin < 2 13 | v_0_min = [0.0 0.5 1.0] ; 14 | end 15 | 16 | if nargin < 3 17 | v_0_max = [0.5 1.0 1.5] ; 18 | end 19 | 20 | % figure out which interval the given v_0 is in 21 | idxs = (v_0 >= v_0_min) & (v_0 <= v_0_max) ; 22 | idx = find(idxs,1,'last') ; 23 | 24 | % return the corresponding v_0 range 25 | v_0_range = nan(1,2) ; 26 | v_0_range(1) = v_0_min(idx) ; 27 | v_0_range(2) = v_0_max(idx) ; 28 | end -------------------------------------------------------------------------------- /step_3_FRS_computation/functions/plot_turtlebot_FRS_in_world_frame.m: -------------------------------------------------------------------------------- 1 | function h = plot_turtlebot_FRS_in_world_frame(FRS,k,pose,varargin) 2 | % plot_turtlebot_FRS_in_world_frame(FRS,k,pose,'property1',value1,'property2',value2,...) 3 | % 4 | % Given an FRS structure output by compute_turtlebot_FRS.m, a choice of k, 5 | % and a turtlebot pose (x,y,h), plot the FRS corresponding to k at that 6 | % pose. 7 | % 8 | % Note that k should be a 2-by-1, with values between -1 and 1. 9 | % 10 | % This function plots a patch, so it also takes in all the usual patch 11 | % property/value arguments, and will return a patch plot handle if an 12 | % output is specified. 13 | % 14 | % Author: Shreyas Kousik and Sean Vaskov 15 | % Created: 21 Oct 2019 16 | % Updated: 29 Oct 2019 17 | % 18 | % See also: get_2D_contour_points, FRS_to_world 19 | 20 | % evaluate FRS at k 21 | I = FRS.FRS_polynomial ; 22 | I = msubs(I,FRS.k,k) ; 23 | 24 | % generate patch info 25 | [~,FRS_patch_info,N_vertices] = get_2D_contour_points(I,FRS.z,1,'Bounds',0.9) ; 26 | 27 | if ~isempty(FRS_patch_info) 28 | % get the FRS properties needed to plot in world frame 29 | x0 = FRS.initial_x ; y0 = FRS.initial_y ; 30 | if isfield(FRS,'distance_scale_x') && isfield(FRS,'distance_scale_y') 31 | Dx = FRS.distance_scale_x ; 32 | Dy = FRS.distance_scale_y ; 33 | else 34 | Dx = FRS.distance_scale ; 35 | Dy = FRS.distance_scale ; 36 | end 37 | 38 | % find the patch info with the largest number of vertices, because 39 | % that is probably the one that matters 40 | [~,idx] = max(N_vertices) ; 41 | 42 | % get the data for the patch into the world frame 43 | V_idx = FRS_patch_info(idx).Vertices' ; 44 | FRS_patch_info(idx).Vertices = FRS_to_world(V_idx,pose,x0,y0,Dx,Dy)' ; 45 | 46 | % plot the patch 47 | h = patch(FRS_patch_info(idx),varargin{:}) ; 48 | end 49 | 50 | if nargout < 1 51 | clear h 52 | end 53 | end -------------------------------------------------------------------------------- /step_3_FRS_computation/functions/visualize_turtlebot_FRS.m: -------------------------------------------------------------------------------- 1 | function visualize_turtlebot_FRS(FRS_info,k_eval,use_FRS_scaling) 2 | % visualize_turtlebot_FRS(FRS_info,k_eval,use_FRS_scaling) 3 | % 4 | % Visualize the FRS contained in FRS_info by plotting the contour for the 5 | % input k_eval. If the input use_FRS_scaling is omitted or true, then the 6 | % FRS is plotted in its scaled and shifted FRS frame. Otherwise, the 7 | % scaling and shifting is undone, and the FRS is plotted with the initial 8 | % condition at the origin. 9 | % 10 | % Author: Shreyas Kousik 11 | % Created: 20 May 2019 12 | % Updated: 28 Oct 2019 13 | 14 | % parse inputs 15 | if nargin < 3 16 | use_FRS_scaling = true ; 17 | end 18 | 19 | % extract variables 20 | z = FRS_info.z ; 21 | k = FRS_info.k ; 22 | 23 | % extract FRS polynomial 24 | I = FRS_info.FRS_polynomial ; 25 | 26 | % extract initial condition set 27 | h_Z0 = FRS_info.h_Z0 ; 28 | 29 | % extract scaling info 30 | distance_scale = FRS_info.distance_scale ; 31 | initial_x = FRS_info.initial_x ; 32 | initial_y = FRS_info.initial_y ; 33 | 34 | % evaluate I(.,k) 35 | I_z = msubs(I,k,k_eval) ; 36 | 37 | % set up for plot 38 | figure(1) ; clf ; hold on ; axis equal 39 | 40 | if use_FRS_scaling 41 | % plot the FRS 42 | plot_2D_msspoly_contour(I_z,z,1,'LineWidth',1.5,'Color',[0.1 0.8 0.3]) 43 | 44 | % plot the initial condition 45 | plot_2D_msspoly_contour(h_Z0,z,0,'LineWidth',1.5,'Color','b') 46 | else 47 | % plot the FRS 48 | offset = -distance_scale*[initial_x;initial_y] ; 49 | plot_2D_msspoly_contour(I_z,z,1,'LineWidth',1.5,'Color',[0.1 0.8 0.3],... 50 | 'Offset',offset,'Scale',distance_scale) 51 | 52 | % plot the initial condition 53 | plot_2D_msspoly_contour(h_Z0,z,0,'LineWidth',1.5,'Color','b',... 54 | 'Offset',offset,'Scale',distance_scale) 55 | end 56 | end -------------------------------------------------------------------------------- /step_3_FRS_computation/images/image_1_for_example_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/images/image_1_for_example_7.png -------------------------------------------------------------------------------- /step_3_FRS_computation/images/image_2_for_example_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/images/image_2_for_example_7.png -------------------------------------------------------------------------------- /step_3_FRS_computation/images/image_for_example_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/images/image_for_example_6.png -------------------------------------------------------------------------------- /step_3_FRS_computation/images/image_for_example_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/images/image_for_example_8.png -------------------------------------------------------------------------------- /step_3_FRS_computation/images/step_3_ex_1_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/images/step_3_ex_1_img_1.png -------------------------------------------------------------------------------- /step_3_FRS_computation/images/step_3_ex_2_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/images/step_3_ex_2_img_1.png -------------------------------------------------------------------------------- /step_3_FRS_computation/images/step_3_ex_3_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_3_FRS_computation/images/step_3_ex_3_img_1.png -------------------------------------------------------------------------------- /step_3_FRS_computation/scripts/step_3_compute_distance_scale.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script computes the distance scaling needed to compute an FRS for 3 | % the Turtlebot FRS. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 20 May 2019 7 | % Updated: 28 Oct 2019 8 | % 9 | %% user parameters 10 | % uncomment one of the following lines to load the relevant error function 11 | % data; we'll compute the FRS for that error function 12 | 13 | % filename = 'turtlebot_error_functions_v_0_0.0_to_0.5.mat' ; 14 | % filename = 'turtlebot_error_functions_v_0_0.5_to_1.0.mat' ; 15 | % filename = 'turtlebot_error_functions_v_0_1.0_to_1.5.mat' ; 16 | 17 | %% automated from here 18 | % load timing info 19 | load('turtlebot_timing.mat') 20 | 21 | % load error info 22 | load(filename) 23 | 24 | % set max speed 25 | max_speed = 1.5 ; % we'll just hard code this for now 26 | 27 | % create the agent 28 | A = turtlebot_agent ; 29 | 30 | % create the range of command inputs and initial conditions 31 | w_range = [0, w_max] ; 32 | v_range = [-delta_v, delta_v] ; 33 | v_0_range = [v_0_min, v_0_max] ; 34 | 35 | % initialize time and distance scales 36 | time_scale = get_t_f_from_v_0(v_0_min) ; 37 | distance_scale_x = 0 ; 38 | distance_scale_y = 0 ; 39 | 40 | % get the stopping time for the given speed range 41 | t_f = get_t_f_from_v_0(v_0_min) ; 42 | 43 | % iterate through yaw rates, speeds, and initial conditions 44 | for v_0 = v_0_range 45 | for w_des = w_range 46 | v_des_range = v_0 + v_range ; 47 | 48 | for v_des_cur = v_des_range 49 | % make sure we're not above the max speed 50 | v_des = bound_values(v_des_cur,[0, max_speed]) ; 51 | 52 | % reset the agent 53 | z0 = [0;0;0;v_0] ; 54 | A.reset(z0) ; 55 | 56 | % create the trajectory to realize 57 | [T,U,Z] = make_turtlebot_desired_trajectory(t_f,w_des,v_des) ; 58 | 59 | % run the agent 60 | A.move(T(end),T,U,Z) ; 61 | 62 | % find the distance traveled 63 | distance_scale_x = max(distance_scale_x,max(A.state(A.position_indices(1),:))) ; 64 | distance_scale_y = max(distance_scale_y,2*max(A.state(A.position_indices(2),:))) ; 65 | end 66 | end 67 | end 68 | 69 | % find the distance scale 70 | distance_scale = max(distance_scale_x,distance_scale_y) + 2*A.footprint ; 71 | disp(['Distance scale found: ',num2str(distance_scale,'%0.2f')]) 72 | 73 | % set the (x0,y0) offset so that x0 lies at -0.5, since the dynamics are 74 | % scaled to travel a distance of around 1 75 | initial_x = -0.5 ; 76 | initial_y = 0 ; 77 | 78 | %% save output 79 | filename = ['turtlebot_FRS_scaling_v_0_',... 80 | num2str(v_0_min,'%0.1f'),'_to_',... 81 | num2str(v_0_max,'%0.1f'),'.mat'] ; 82 | save(filename,'time_scale','distance_scale',... 83 | 'distance_scale_x','distance_scale_y',... 84 | 'max_speed',... 85 | 'initial_x','initial_y') ; 86 | -------------------------------------------------------------------------------- /step_3_FRS_computation/scripts/step_3_verify_distance_scale.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script demonstrates how to use the distance scale correctly to get 3 | % points in and out of the FRS frame. The functions being checked are: 4 | % world_to_FRS_frame 5 | % FRS_to_world_frame 6 | % 7 | % Note that this script requires the FRS .mat files computed in Step 3. 8 | % 9 | % Author: Shreyas Kousik 10 | % Created: 29 May 2019 11 | % Updated: - 12 | % 13 | %% user parameters 14 | % agent pose in SE(2) 15 | pose = [0;0.5;1] ; % (x,y,h) 16 | 17 | % test point in world frame to move to FRS frame 18 | p_world = [0;1] ; 19 | 20 | % test point in FRS frame to move to world frame 21 | p_FRS = [0;0] ; 22 | 23 | % FRS to use 24 | FRS = load('turtlebot_FRS_deg_4_v_0_0.5_to_1.0.mat') ; 25 | 26 | %% automated from here 27 | % create turtlebot 28 | A = turtlebot_agent ; 29 | A.reset([pose;0]) 30 | 31 | %% move the world footprint to the FRS frame 32 | % get some points from the agent footprint 33 | P_fp_world = A.footprint_vertices(:,1:5:end) ; 34 | 35 | % move the agent footprint to the global frame 36 | N_fp = size(P_fp_world,2) ; 37 | P_fp_world = P_fp_world + repmat(A.state(A.position_indices,end),1,N_fp) ; 38 | 39 | % move actual footprint to FRS frame 40 | x0 = FRS.initial_x ; 41 | y0 = FRS.initial_y ; 42 | D = FRS.distance_scale ; 43 | P_fp_FRS = world_to_FRS(P_fp_world,A.state(:,end),x0,y0,D) ; 44 | 45 | %% move the FRS footprint to the world frame 46 | % get footprint in FRS frame 47 | h_Z0 = FRS.h_Z0 ; 48 | z = FRS.z ; 49 | P_Z0_FRS = get_2D_contour_points(h_Z0,z,0) ; 50 | 51 | % move FRS frame footprint to world frame 52 | P_Z0_world = FRS_to_world(P_Z0_FRS(:,1:5:end),A.state(:,end),x0,y0,D) ; 53 | 54 | %% move the world point to the FRS frame and back 55 | p_world_to_FRS = world_to_FRS(p_world,A.state(:,end),x0,y0,D) ; 56 | p_world_to_FRS_to_world = FRS_to_world(p_world_to_FRS,A.state(:,end),x0,y0,D) ; 57 | 58 | %% move the FRS point to the world frame and back 59 | p_FRS_to_world = FRS_to_world(p_FRS,A.state(:,end),x0,y0,D) ; 60 | p_FRS_to_world_to_FRS = world_to_FRS(p_FRS_to_world,A.state(:,end),x0,y0,D) ; 61 | 62 | %% plotting (to verify) 63 | figure(1) ; clf ; 64 | 65 | %% plot FRS frame on left 66 | subplot(1,2,1) ; hold on ; axis equal ; grid on ; 67 | 68 | % plot FRS frame footprint 69 | plot(P_Z0_FRS(1,:),P_Z0_FRS(2,:),'b','LineWidth',1.5) 70 | 71 | % plot world_to_local footprint 72 | plot(P_fp_FRS(1,:),P_fp_FRS(2,:),'b*') 73 | 74 | % plot the FRS point 75 | plot(p_FRS(1,:),p_FRS(2,:),'r*','MarkerSize',12) 76 | plot(p_FRS_to_world_to_FRS(1,:),p_FRS_to_world_to_FRS(2,:),'ro','MarkerSize',12) 77 | 78 | % plot the world point moved into the FRS frame 79 | plot(p_world_to_FRS(1,:),p_world_to_FRS(2,:),'k*','MarkerSize',12) 80 | 81 | % labeling 82 | title('FRS Frame') 83 | set(gca,'FontSize',15) 84 | 85 | %% plot world frame on right 86 | subplot(1,2,2) ; hold on ; axis equal ; grid on ; 87 | 88 | % plot agent 89 | plot(A) 90 | 91 | % plot footprint from FRS frame 92 | plot(P_Z0_world(1,:),P_Z0_world(2,:),'b*') 93 | 94 | % plot the world point 95 | plot(p_world(1,:),p_world(2,:),'k*','MarkerSize',12) 96 | plot(p_world_to_FRS_to_world(1,:),p_world_to_FRS_to_world(2,:),'ko','MarkerSize',12) 97 | 98 | % plot the FRS point moved into the world frame 99 | plot(p_FRS_to_world(1,:),p_FRS_to_world(2,:),'r*','MarkerSize',12) 100 | 101 | % labeling 102 | title('World Frame') 103 | set(gca,'FontSize',15) -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/034d0a6be0424bffe9a6e7ac9236c0f5.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/190083ef7a1625fbc75f243cffb9c96d.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/21fd4e8eecd6bdf1a4d3d6bd1fb8d733.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/27e556cf3caa0673ac49a8f0de3c73ca.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/2db35060f2b6f146752157657cfb5d5a.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/2f118ee06d05f3c2d98361d9c30e38ce.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/332cc365a4987aacce0ead01b8bdcc0b.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/3cf4fbd05970446973fc3d9fa3fe3c41.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/5b51bd2e6f329245d425b8002d7cf942.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/63bb9849783d01d91403bc9a5fea12a2.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/699628c77c65481a123e3649944c0d51.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/6c4adbc36120d62b98deef2a20d5d303.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/9f0028b414617caf75a357cfb98e7497.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/a9a3a4a202d80326bda413b5562d5cd1.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/ae4fb5973f393577570881fc24fc2054.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/b8bc815b5e9d5177af01fd4d3d3c2f10.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/c0463eeb4772bfde779c20d52901d01b.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/db85bd6dfbbcc6817fc7960910b43296.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/deceeaf6940a8c7a5a02373728002b0f.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/eb4779c5fded13881cb5f169b1f10c73.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /step_3_FRS_computation/tex/f93ce33e511096ed626b4719d50f17d2.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_4_online_planning/functions/compute_turtlebot_discretized_obs.m: -------------------------------------------------------------------------------- 1 | function [O_FRS, O_buf_out, O_pts_out] = compute_turtlebot_discretized_obs(O_world,turtlebot_pose,b,r,FRS,O_bounds) 2 | % O_FRS = compute_turtlebot_discretized_obs(O_world,turtlebot_pose,b,r,FRS) 3 | % O_FRS = compute_turtlebot_discretized_obs(O_world,turtlebot_pose,b,r,FRS,O_bounds) 4 | % 5 | % Take obstacle points defined in a global coordinate frame and transform 6 | % them into the scaled, shifted FRS frame for the TurtleBot. 7 | % 8 | % Author: Shreyas Kousik 9 | % Created: 30 May 2019 10 | 11 | % for now, just don't use the arc point spacing by setting the miter limit 12 | % to 2 in buffer_polygon_obstacles 13 | O_buf = buffer_polygon_obstacles(O_world,b,2) ; 14 | 15 | % handle the world bounds obstacle if it's passed in 16 | if nargin > 5 17 | O_bounds_buf = buffer_polygon_obstacles(O_bounds,b,2) ; 18 | O_buf = [O_buf, nan(2,1), O_bounds_buf] ; 19 | end 20 | 21 | % create discretized obstacle points 22 | O_pts = interpolate_polyline_with_spacing(O_buf,r) ; 23 | 24 | % put the obstacles into the FRS frame 25 | x0 = FRS.initial_x ; 26 | y0 = FRS.initial_y ; 27 | D = FRS.distance_scale ; 28 | O_FRS = world_to_FRS(O_pts,turtlebot_pose,x0,y0,D) ; 29 | 30 | % filter out points that are too far away to be reached 31 | O_FRS = crop_points_outside_region(0,0,O_FRS,1) ; 32 | 33 | % return the buffered obstacle as well 34 | if nargout > 1 35 | O_buf_out = O_buf ; 36 | if nargout > 2 37 | O_pts_out = O_pts ; 38 | end 39 | end 40 | end -------------------------------------------------------------------------------- /step_4_online_planning/functions/compute_turtlebot_point_spacings.m: -------------------------------------------------------------------------------- 1 | function [r,a] = compute_turtlebot_point_spacings(R,b) 2 | % [r,a] = compute_turtlebot_point_spacings(R,b) 3 | % 4 | % Compute the point spacing r and arc point spacing a, as per Example 67 5 | % on page 35 of the Big Paper (https://arxiv.org/abs/1809.06746). These are 6 | % used to discretize obstacles for online planning. 7 | % 8 | % Author: Shreyas Kousik 9 | % Created: 30 May 2019 10 | % Updated: 29 Oct 2019 11 | 12 | % first, make sure the buffer is valid 13 | bbar = R ; 14 | 15 | if b > bbar 16 | disp('Resizing obstacle buffer to be valid!') 17 | b = bbar - 0.01 ; 18 | end 19 | 20 | % now compute the point spacing r and arc point spacing a 21 | theta_1 = acos((R-b)/R) ; 22 | theta_2 = acos(b/(2*R)) ; 23 | r = 2*R*sin(theta_1) ; 24 | 25 | if nargout > 1 26 | a = 2*b*sin(theta_2) ; 27 | end 28 | end -------------------------------------------------------------------------------- /step_4_online_planning/functions/compute_turtlebot_trajopt_constraints.m: -------------------------------------------------------------------------------- 1 | function [A_ineq,b_ineq,nonlcon] = make_turtlebot_trajopt_constraints(FRS,I,O_FRS,initial_speed) 2 | 3 | %% create the linear constraint matrices 4 | 5 | %% create the nonlinear constraint function 6 | 7 | 8 | % create the 9 | 10 | end -------------------------------------------------------------------------------- /step_4_online_planning/functions/turtlebot_cost.m: -------------------------------------------------------------------------------- 1 | function cost = turtlebot_cost(k_1,k_2,w_max,v_max,x_des,y_des) 2 | %TURTLEBOT_COST 3 | % COST = TURTLEBOT_COST(K_1,K_2,W_MAX,V_MAX,X_DES,Y_DES) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 30-May-2019 17:42:14 7 | 8 | t2 = v_max.*(9.0./4.0e2); 9 | t3 = k_2.*v_max.*(9.0./4.0e2); 10 | t4 = t2+t3; 11 | t5 = k_1.^2; 12 | t6 = w_max.^2; 13 | t7 = v_max.*(9.0./2.0e2); 14 | t8 = k_2.*v_max.*(9.0./2.0e2); 15 | t9 = t7+t8; 16 | t10 = k_1.*t9.*w_max.*(1.0./2.0e1); 17 | t11 = k_1.*t4.*w_max.*(1.0./2.0e1); 18 | t12 = t10+t11; 19 | t13 = v_max.*(2.7e1./4.0e2); 20 | t14 = k_2.*v_max.*(2.7e1./4.0e2); 21 | t17 = t4.*t5.*t6.*2.25e-3; 22 | t15 = t13+t14-t17; 23 | t16 = k_1.*t15.*w_max.*(1.0./2.0e1); 24 | t18 = k_1.*t12.*w_max.*(9.0./2.0e2); 25 | t19 = t10+t11+t16; 26 | t20 = k_1.*t19.*w_max.*(9.0./2.0e2); 27 | t21 = v_max.*(9.0./1.0e2); 28 | t22 = k_2.*v_max.*(9.0./1.0e2); 29 | t23 = t17+t18-t21-t22; 30 | t29 = v_max.*(9.0./8.0e1); 31 | t30 = k_2.*v_max.*(9.0./8.0e1); 32 | t24 = t17+t18+t20-t29-t30; 33 | t28 = k_1.*t23.*w_max.*(1.0./2.0e1); 34 | t25 = t10+t11+t16-t28; 35 | t26 = k_1.*t25.*w_max.*(9.0./2.0e2); 36 | t31 = v_max.*(2.7e1./2.0e2); 37 | t32 = k_2.*v_max.*(2.7e1./2.0e2); 38 | t27 = t17+t18+t20+t26-t31-t32; 39 | t33 = k_1.*t27.*w_max.*(1.0./2.0e1); 40 | t34 = k_1.*t24.*w_max.*(1.0./2.0e1); 41 | t35 = t10+t11+t16-t28-t34; 42 | t36 = k_1.*t35.*w_max.*(9.0./2.0e2); 43 | t43 = v_max.*(6.3e1./4.0e2); 44 | t44 = k_2.*v_max.*(6.3e1./4.0e2); 45 | t37 = t17+t18+t20+t26+t36-t43-t44; 46 | t38 = k_1.*t37.*w_max.*(1.0./2.0e1); 47 | t39 = t10+t11+t16-t28-t33-t34; 48 | t40 = k_1.*t39.*w_max.*(9.0./2.0e2); 49 | t46 = v_max.*(9.0./5.0e1); 50 | t47 = k_2.*v_max.*(9.0./5.0e1); 51 | t41 = t17+t18+t20+t26+t36+t40-t46-t47; 52 | t42 = k_1.*t41.*w_max.*(1.0./2.0e1); 53 | t48 = v_max.*(8.1e1./4.0e2); 54 | t49 = k_2.*v_max.*(8.1e1./4.0e2); 55 | t50 = -t10-t11-t16+t28+t33+t34+t38; 56 | t51 = k_1.*t50.*w_max.*(9.0./2.0e2); 57 | t52 = t17+t18+t20+t26+t36+t40-t48-t49-t51; 58 | t53 = k_1.*t52.*w_max.*(1.0./2.0e1); 59 | t45 = t17+t18+t20+t26-v_max.*(9.9e1./4.0e2)+x_des-k_2.*v_max.*(9.9e1./4.0e2)+k_1.*w_max.*(t10+t11+t16-k_1.*t23.*w_max.*(1.0./2.0e1)-k_1.*t24.*w_max.*(1.0./2.0e1)-k_1.*t27.*w_max.*(1.0./2.0e1)).*(9.0./2.0e2)-k_1.*w_max.*(-t10-t11-t16+t28+t33+t34+t38+t42).*(9.0./2.0e2)-k_1.*w_max.*(-t10-t11-t16+t28+t33+t34+k_1.*w_max.*(t17+t18+t20+t26-v_max.*(6.3e1./4.0e2)-k_2.*v_max.*(6.3e1./4.0e2)+k_1.*w_max.*(t10+t11+t16-t28-k_1.*t24.*w_max.*(1.0./2.0e1)).*(9.0./2.0e2)).*(1.0./2.0e1)).*(9.0./2.0e2)+k_1.*w_max.*(t10+t11+t16-k_1.*t24.*w_max.*(1.0./2.0e1)+k_1.*w_max.*(t21+t22-k_1.*t12.*w_max.*(9.0./2.0e2)-t4.*t5.*t6.*2.25e-3).*(1.0./2.0e1)).*(9.0./2.0e2)-k_1.*w_max.*(-t10-t11-t16+t28+t33+t34+t38+t42+t53).*(9.0./2.0e2); 60 | t54 = k_1.*w_max.*(t17+t18-t21-t22).*(1.0./2.0e1); 61 | t55 = -t10-t11-t16+t33+t34+t38+t42+t53+t54+y_des+k_1.*w_max.*(t17+t18+t20+t26+t36+t40-t51-v_max.*(9.0./4.0e1)-k_2.*v_max.*(9.0./4.0e1)-k_1.*w_max.*(-t10-t11-t16+t33+t34+t38+t42+t54).*(9.0./2.0e2)).*(1.0./2.0e1); 62 | cost = t45.^2+t55.^2; 63 | -------------------------------------------------------------------------------- /step_4_online_planning/functions/turtlebot_cost_for_fmincon.m: -------------------------------------------------------------------------------- 1 | function [c, gc] = turtlebot_cost_for_fmincon(k,FRS,wp_local,start_tic,timeout) 2 | % [c, gc] = turtlebot_cost_for_fmincon(k,FRS,wp_local) 3 | % [c, gc] = turtlebot_cost_for_fmincon(k,FRS,wp_local,start_tic,timeout) 4 | % 5 | % Evaluate the cost and cost gradient for use with fmincon when performing 6 | % trajectory optimization for the TurtleBot. 7 | % 8 | % Author: Shreyas Kousik 9 | % Created: 31 May 2019 10 | % 11 | % See also: turtlebot_cost, turtlebot_cost_grad, 12 | % turtlebot_nonlcon_for_fmincon 13 | 14 | % evaluate cost and gradient 15 | c = turtlebot_cost(k(1),k(2),FRS.w_max,FRS.v_range(2),wp_local(1),wp_local(2)) ; 16 | gc = turtlebot_cost_grad(k(1),k(2),FRS.w_max,FRS.v_range(2),wp_local(1),wp_local(2)) ; 17 | 18 | % perform timeout check 19 | if nargin > 3 && toc(start_tic) > timeout 20 | error('Timed out while evaluating cost function!') 21 | end 22 | end -------------------------------------------------------------------------------- /step_4_online_planning/functions/turtlebot_nonlcon_for_fmincon.m: -------------------------------------------------------------------------------- 1 | function [n, neq, gn, gneq] = turtlebot_nonlcon_for_fmincon(k,... 2 | cons_poly,cons_poly_grad,start_tic,timeout) 3 | 4 | % [n, neq, gn, gneq] = turtlebot_nonlcon_for_fmincon(k,... 5 | % cons_poly,cons_poly_grad,start_tic,timeout) 6 | % 7 | % Evaluate the constraint Jacobian for use with fmincon when performing 8 | % trajectory optimization for the TurtleBot. 9 | % 10 | % Author: Shreyas Kousik 11 | % Created: 31 May 2019 12 | % 13 | % See also: turtlebot_cost, turtlebot_cost_grad, 14 | % turtlebot_nonlcon_for_fmincon, evaluate_constraint_polynomial, 15 | % evaluate_constraint_polynomial_gradient 16 | 17 | n = evaluate_constraint_polynomial(cons_poly,k) ; 18 | neq = [] ; 19 | gn = evaluate_constraint_polynomial_gradient(cons_poly_grad,k)' ; 20 | gneq = [] ; 21 | 22 | % perform timeout check 23 | if nargin > 3 && toc(start_tic) > timeout 24 | error('Timed out while evaluating cost function!') 25 | end 26 | end -------------------------------------------------------------------------------- /step_4_online_planning/functions/turtlebot_symbolic_traj_prod_model.m: -------------------------------------------------------------------------------- 1 | function zd = turtlebot_symbolic_traj_prod_model(z,v_des,w_des) 2 | % zd = turtlebot_symbolic_traj_prod_model(z,v_des,w_des) 3 | % 4 | % Output the dynamics of the TurtleBot's trajectory-producing model 5 | % that can be used with symbolic variables. 6 | % 7 | % Author: Shreyas Kousik 8 | % Created: 30 May 2019 9 | 10 | % extract states 11 | x = z(1) ; 12 | y = z(2) ; 13 | 14 | % compute dynamics 15 | zd = [v_des - w_des*y ; 16 | w_des*x ] ; 17 | end -------------------------------------------------------------------------------- /step_4_online_planning/images/step_4_ex_1_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_4_online_planning/images/step_4_ex_1_img_1.png -------------------------------------------------------------------------------- /step_4_online_planning/images/step_4_ex_1_img_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_4_online_planning/images/step_4_ex_1_img_2.png -------------------------------------------------------------------------------- /step_4_online_planning/images/step_4_ex_2_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skousik/RTD_tutorial/feb8dfca78d08a7b726e1554be61987841035132/step_4_online_planning/images/step_4_ex_2_img_1.png -------------------------------------------------------------------------------- /step_4_online_planning/scripts/step_4_run_turtlebot_simulation.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script runs a simulation with the TurtleBot in the simulator 3 | % framework, using RTD to plan online. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 6 Jun 2019 7 | % Updated: 8 Mar 2020 8 | % 9 | %% user parameters 10 | % world 11 | obstacle_size_bounds = [0.2, 0.3] ; % side length [min, max] 12 | N_obstacles = 12 ; 13 | bounds = [-4,4,-2,2] ; 14 | goal_radius = 0.5 ; 15 | 16 | % planner 17 | buffer = 0.01 ; % m 18 | t_plan = 0.5 ; % if t_plan = t_move, then real time planning is enforced 19 | t_move = 0.5 ; 20 | FRS_degree = 12 ; 21 | plot_FRS_flag = true ; 22 | plot_HLP_flag = true ; 23 | 24 | % simulation 25 | verbose_level = 5 ; 26 | max_sim_time = 1000 ; 27 | max_sim_iterations = 1000 ; 28 | 29 | % plotting 30 | plot_while_simulating_flag = true ; 31 | animate_after_simulating_flag = true ; 32 | 33 | %% automated from here 34 | A = turtlebot_agent ; 35 | 36 | P = turtlebot_RTD_planner_static('verbose',verbose_level,'buffer',buffer,... 37 | 't_plan',t_plan,'t_move',t_move,... 38 | 'FRS_degree',FRS_degree,... 39 | 'plot_FRS_flag',plot_FRS_flag,... 40 | 'plot_HLP_flag',plot_HLP_flag) ; 41 | 42 | P.HLP = RRT_star_HLP() ; 43 | 44 | W = static_box_world('bounds',bounds,'N_obstacles',N_obstacles,'buffer',0.25,... 45 | 'verbose',verbose_level,'goal_radius',goal_radius,... 46 | 'obstacle_size_bounds',obstacle_size_bounds) ; 47 | 48 | S = simulator(A,W,P,'allow_replan_errors',true,'verbose',verbose_level,... 49 | 'max_sim_time',max_sim_time,... 50 | 'max_sim_iterations',max_sim_iterations,... 51 | 'plot_while_running',plot_while_simulating_flag) ; 52 | 53 | %% run simulation 54 | S.run() ; 55 | 56 | %% animate simulation 57 | if animate_after_simulating_flag 58 | S.animate() 59 | end 60 | -------------------------------------------------------------------------------- /step_4_online_planning/tex/21fd4e8eecd6bdf1a4d3d6bd1fb8d733.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_4_online_planning/tex/4d1719a25e4b38db1fbea7ddf5f1a886.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /step_4_online_planning/tex/5b51bd2e6f329245d425b8002d7cf942.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_4_online_planning/tex/63bb9849783d01d91403bc9a5fea12a2.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_4_online_planning/tex/89f2e0d2d24bcf44db73aab8fc03252c.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_4_online_planning/tex/ac4bb7b235a967476d0ec663bb9588c2.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_4_online_planning/tex/f93ce33e511096ed626b4719d50f17d2.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /step_5_extras/README.md: -------------------------------------------------------------------------------- 1 | # Extras 2 | 3 | ##### [Go back to tutorial home page](https://github.com/skousik/RTD_tutorial) 4 | 5 | In this section, we present more details about implementing RTD: 6 | 7 | 1. [An overview of the simulator framework](https://github.com/skousik/RTD_tutorial/tree/master/step_5_extras/extra_1_simulator_overview) 8 | 2. [How to write an RTD planner object in MATLAB](https://github.com/skousik/RTD_tutorial/tree/master/step_5_extras/extra_2_writing_an_RTD_planner) 9 | 3. [How to write an RRT* planner for the turtlebot in MATLAB](https://github.com/skousik/RTD_tutorial/tree/master/step_5_extras/extra_3_writing_an_RRT_star_planner) 10 | 11 | -------------------------------------------------------------------------------- /step_5_extras/extra_1_simulator_overview/README.md: -------------------------------------------------------------------------------- 1 | # Extras 1: Simulator Overview 2 | 3 | [Go to tutorial home page](https://github.com/skousik/RTD_tutorial) 4 | 5 | Recall that the [simulator](https://github.com/skousik/simulator) repository is needed for online planning with RTD. This section presents a brief overview of that repository. 6 | 7 | The `simulator` requires three things to run. An `agent`, which is a representation of a robot; a `world` full of obstacles that the agent lives in; and a `planner`, which generates a reference trajectory for the agent to avoid obstacles while trying to reach some goal location in the world. 8 | 9 | Each agent, world, and planner is implemented as a MATLAB class. They should inherit the `agent`, `world`, and `planner` superclasses, respectively (or inherit subclasses thereof). In MATLAB pseudocode, the general way we use the simulator is as follows: 10 | 11 | ```matlab 12 | % set up agent, world, and planner 13 | A = agent_subclass('property1',value1,'property2',value2,...) ; 14 | W = world_subclass('property1',value1,'property2',value2,...) ; 15 | P = planner_subclass('property1',value1,'property2',value2,...) ; 16 | S = simulator(A,W,P,'property1',value1,'property2',value2,...) 17 | 18 | % run simulation 19 | S.run() 20 | ``` 21 | 22 | We have built this framework to run simulation comparisons of various planner types in a variety of worlds, given a single robot. So, instead of just passing in one planner and one world, you can also pass in a cell array of planners and worlds. As long as they all share certain required properties and methods, the simulator will run just fine. 23 | 24 | In particular, the planner is expected to be a **receding-horizon planner**. This means that the planner creates a new, short plan while the agent executes a previously-found plan for a small duration. This strategy lets the agent learn about the world around it (i.e., sense obstacles) as it replans. 25 | 26 | This writeup will review the properties and methods of the simulator, agent, world, and planner classes. To see how these are used, check out the rest of the RTD tutorial. In particular, check out the `run_turtlebot_simulation.m` script in [Step 4: Online Planning](https://github.com/skousik/RTD_tutorial/tree/master/step_4_online_planning#44-running-a-simulation). 27 | 28 | 29 | 30 | ## Extras 1.1: Simulator 31 | 32 | ### Properties 33 | 34 | Coming soon! 35 | 36 | ### Methods 37 | 38 | Coming soon! 39 | 40 | 41 | 42 | ## Extras 1.2: Agent 43 | 44 | ### Properties 45 | 46 | Coming soon! 47 | 48 | ### Methods 49 | 50 | Coming soon! 51 | 52 | 53 | 54 | ## Extras 1.3: World 55 | 56 | ### Properties 57 | 58 | Coming soon! 59 | 60 | ### Methods 61 | 62 | Coming soon! 63 | 64 | 65 | 66 | ## Extras 1.4: Planner 67 | 68 | ### Properties 69 | 70 | Coming soon! 71 | 72 | ### Methods 73 | 74 | Coming soon! 75 | 76 | -------------------------------------------------------------------------------- /step_5_extras/extra_2_writing_an_RTD_planner/extra_2_run_my_turtlebot_planner_simulation.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script runs a simulation with the TurtleBot in the simulator 3 | % framework, using RTD to plan online. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 6 June 2019 7 | % Updated: 7 June 2019 8 | % 9 | %% user parameters 10 | % world 11 | obstacle_size_bounds = [0.2, 0.3] ; % side length [min, max] 12 | N_obstacles = 7 ; 13 | bounds = [-4,4,-2,2] ; 14 | goal_radius = 0.5 ; 15 | 16 | % planner 17 | buffer = 0.05 ; % m 18 | t_plan = 0.5 ; % if t_plan = t_move, then real time planning is enforced 19 | t_move = 0.5 ; 20 | 21 | % simulation 22 | verbose_level = 10 ; 23 | 24 | %% automated from here 25 | A = turtlebot_agent ; 26 | 27 | P = my_turtlebot_RTD_planner('verbose',verbose_level,'buffer',buffer,... 28 | 't_plan',t_plan,'t_move',t_move) ; 29 | 30 | W = static_box_world('bounds',bounds,'N_obstacles',N_obstacles,'buffer',0.25,... 31 | 'verbose',verbose_level,'goal_radius',goal_radius,... 32 | 'obstacle_size_bounds',obstacle_size_bounds) ; 33 | 34 | S = simulator(A,W,P,'allow_replan_errors',true,'verbose',verbose_level,... 35 | 'max_sim_time',30,'max_sim_iterations',60) ; 36 | 37 | %% run simulation 38 | S.run ; -------------------------------------------------------------------------------- /step_5_extras/extra_3_writing_an_RRT_star_planner/run_turtlebot_simulation_RRT_star_planner.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script runs a simulation with the TurtleBot in the simulator 3 | % framework, using RRT to plan online. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 19 Oct 2019 7 | % Updated: 24 Jan 2019 8 | % 9 | %% user parameters 10 | % agent 11 | desired_speed = 0.75 ; % m/s 12 | sensor_radius = 2.5 ; % make this larger if initialize_tree_mode is 'once' 13 | 14 | % world 15 | obstacle_size_bounds = [0.2, 0.3] ; % side length [min, max] 16 | N_obstacles = 7 ; 17 | bounds = [-4,4,-2,2] ; 18 | goal_radius = 0.5 ; 19 | world_buffer = 0.25 ; % this is used to make start and goal locations that are not too close to the robot or boundary 20 | 21 | % planner 22 | buffer = 0.3 ; % [m] distance used to buffer obstacles for planning, must be larger than the agent's footprint 23 | t_plan = 1.0 ; % if t_plan = t_move, then real time planning is enforced 24 | t_move = 0.5 ; 25 | initialize_tree_mode = 'iter' ; % 'iter' or 'once' 26 | HLP_grow_tree_mode = 'new' ; % 'new' or 'seed' or 'keep' (only matters if using 'iter' above) 27 | grow_tree_once_timeout = 10 ; 28 | HLP_type = 'rrt' ; % 'rrt' or 'rrt*' or 'connect' or 'connect*' 29 | new_node_growth_distance = 0.5 ; 30 | 31 | % plotting 32 | plot_HLP_flag = true ; 33 | plot_tree_growth_flag = true ; 34 | 35 | % simulation 36 | verbose_level = 5 ; 37 | max_sim_iterations = 100 ; 38 | max_sim_time = 100 ; 39 | 40 | %% automated from here 41 | A = turtlebot_agent('sensor_radius',sensor_radius) ; 42 | 43 | % this is needed for the agent to track the RRT* output 44 | A.LLC.gains.yaw = 10 ; 45 | A.LLC.lookahead_time = 0.1 ; 46 | 47 | P = turtlebot_RRT_planner('verbose',verbose_level,'buffer',buffer,... 48 | 't_plan',t_plan,'t_move',t_move,'desired_speed',desired_speed,... 49 | 'HLP_type',HLP_type,... 50 | 'initialize_tree_mode',initialize_tree_mode,... 51 | 'grow_tree_once_timeout',grow_tree_once_timeout,... 52 | 'HLP_grow_tree_mode',HLP_grow_tree_mode) ; 53 | 54 | P.HLP.new_node_growth_distance = new_node_growth_distance ; 55 | P.plot_HLP_flag = plot_HLP_flag ; 56 | P.HLP.plot_while_growing_tree_flag = plot_tree_growth_flag ; 57 | 58 | W = static_box_world('bounds',bounds,'N_obstacles',N_obstacles,'buffer',world_buffer,... 59 | 'verbose',verbose_level,'goal_radius',goal_radius,... 60 | 'obstacle_size_bounds',obstacle_size_bounds) ; 61 | 62 | S = simulator(A,W,P,'allow_replan_errors',true,'verbose',verbose_level,... 63 | 'max_sim_time',max_sim_time,'max_sim_iterations',max_sim_iterations) ; 64 | 65 | 66 | %% set up plot 67 | if strcmpi(initialize_tree_mode,'once') 68 | figure(1) ; clf ; axis equal ; hold on ; 69 | plot(W) 70 | end 71 | 72 | %% run simulation 73 | S.run() ; -------------------------------------------------------------------------------- /step_5_extras/extra_3_writing_an_RRT_star_planner/testing_RRT_HLP.m: -------------------------------------------------------------------------------- 1 | %% description 2 | % This script demonstrates the RRT high-level planner variants in a static 3 | % box world instance. 4 | % 5 | % Author: Shreyas Kousik 6 | % Created: 30 Oct 2019 7 | % Updated: 6 Nov 2019 8 | 9 | clear ; 10 | 11 | %% user parameters 12 | % agent 13 | sensor_radius = 10 ; 14 | 15 | % world 16 | obstacle_size_bounds = [0.2, 0.3] ; % side length [min, max] 17 | N_obstacles = 12 ; 18 | bounds = [-4,4,-2,2] ; 19 | goal_radius = 0.5 ; 20 | 21 | % high-level planner 22 | HLP_type = 'rrt*' ; % choose 'RRT' or 'RRT*' or 'connect' or 'connect*' 23 | timeout = 20 ; 24 | 25 | % simulation 26 | verbose_level = 5 ; 27 | max_sim_time = 1000 ; 28 | max_sim_iterations = 1000 ; 29 | 30 | % plotting 31 | plot_tree_flag = true ; 32 | plot_best_path_flag = true ; 33 | plot_while_growing_tree_flag = true ; 34 | 35 | %% automated from here 36 | % create dummy agent (the HLP needs agent_info) 37 | A = agent() ; 38 | 39 | switch lower(HLP_type) 40 | case 'rrt' 41 | HLP = RRT_HLP('timeout',timeout,'plot_best_path_flag',plot_best_path_flag,... 42 | 'plot_tree_flag',plot_tree_flag,'plot_while_growing_tree_flag',plot_while_growing_tree_flag) ; 43 | case 'connect' 44 | HLP = RRT_connect_HLP('timeout',timeout,'plot_best_path_flag',plot_best_path_flag,... 45 | 'plot_tree_flag',plot_tree_flag,'plot_while_growing_tree_flag',plot_while_growing_tree_flag) ; 46 | case 'rrt*' 47 | HLP = RRT_star_HLP('timeout',timeout,'plot_best_path_flag',plot_best_path_flag,... 48 | 'plot_tree_flag',plot_tree_flag,'plot_while_growing_tree_flag',plot_while_growing_tree_flag) ; 49 | case 'connect*' 50 | HLP = RRT_star_connect_HLP('timeout',timeout,'plot_best_path_flag',plot_best_path_flag,... 51 | 'plot_tree_flag',plot_tree_flag,'plot_while_growing_tree_flag',plot_while_growing_tree_flag) ; 52 | otherwise 53 | error('Invalid HLP type!') 54 | end 55 | 56 | 57 | W = static_box_world('bounds',bounds,'N_obstacles',N_obstacles,'buffer',0.25,... 58 | 'verbose',verbose_level,'goal_radius',goal_radius,... 59 | 'obstacle_size_bounds',obstacle_size_bounds) ; 60 | 61 | A.reset(W.start) 62 | A.sensor_radius = sensor_radius ; 63 | 64 | % setup to run RRT* 65 | HLP.grow_tree_mode = 'keep' ; 66 | HLP.bounds = W.bounds ; 67 | HLP.goal = W.goal ; 68 | AI = A.get_agent_info() ; 69 | WI = W.get_world_info(AI) ; 70 | HLP.setup(AI,WI) ; 71 | 72 | %% run RRT* 73 | if plot_while_growing_tree_flag 74 | figure(1) ; clf ; hold on ; axis equal 75 | plot(W) 76 | end 77 | 78 | HLP.grow_tree(AI,WI) ; 79 | 80 | %% plotting 81 | figure(1) ; clf ; hold on ; axis equal 82 | plot(W) 83 | plot(HLP) -------------------------------------------------------------------------------- /tex/27e556cf3caa0673ac49a8f0de3c73ca.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /tex/2db35060f2b6f146752157657cfb5d5a.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /tex/44bc9d542a92714cac84e01cbbb7fd61.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /tex/6c4adbc36120d62b98deef2a20d5d303.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /tex/7c74eeb32158ff7c4f67d191b95450fb.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /tex/ae4fb5973f393577570881fc24fc2054.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /tex/cdcac8939f3840cd8cddf40059a4cf58.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | --------------------------------------------------------------------------------