├── .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 |
--------------------------------------------------------------------------------