├── README.md ├── armplanner.m ├── examplestartgoal.txt ├── map1.txt ├── map2.txt ├── planner.cpp ├── planner.mexa64 ├── prm_graph.cpp ├── prm_graph.h ├── rrt_tree.cpp ├── rrt_tree.h └── runtest.m /README.md: -------------------------------------------------------------------------------- 1 | # sampling_based_planners 2 | Implementation of RRT, RRT-connect, RRT*, and PRM in c++ 3 | -------------------------------------------------------------------------------- /armplanner.m: -------------------------------------------------------------------------------- 1 | function[armplan] = armplanner(envmap, armstart, armgoal, planner_id); 2 | 3 | 4 | %call the planner in C 5 | [armplan, armplanlength] = planner(envmap, armstart, armgoal, planner_id); 6 | 7 | -------------------------------------------------------------------------------- /examplestartgoal.txt: -------------------------------------------------------------------------------- 1 | startQ = [pi/2 pi/4 pi/2 pi/4 pi/2]; 2 | goalQ = [pi/8 3*pi/4 pi 0.9*pi 1.5*pi]; -------------------------------------------------------------------------------- /map1.txt: -------------------------------------------------------------------------------- 1 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 3 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 4 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 5 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 6 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 7 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 8 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 9 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 10 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 11 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 12 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 13 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 14 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 15 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 16 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 17 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 18 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 19 | 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 20 | 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 21 | 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 22 | 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 23 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 24 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 25 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 26 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 27 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 28 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 29 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 30 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 31 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 32 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 33 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 34 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 35 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 36 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 37 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 38 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 39 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 40 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 41 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 42 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 43 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 44 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 45 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 46 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 47 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 48 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 49 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 50 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 51 | -------------------------------------------------------------------------------- /map2.txt: -------------------------------------------------------------------------------- 1 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 3 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 4 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 5 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 6 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 7 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 8 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 9 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 10 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 11 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 12 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 13 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 14 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 15 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 16 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 17 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 18 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 19 | 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 20 | 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 21 | 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 22 | 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 23 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 24 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 25 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 26 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 27 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 28 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 29 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 30 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 31 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 32 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 33 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 34 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 35 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 36 | 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 37 | 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 38 | 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 39 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 40 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 41 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 42 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 43 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 44 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 45 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 46 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 47 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 48 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 49 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 50 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 51 | -------------------------------------------------------------------------------- /planner.cpp: -------------------------------------------------------------------------------- 1 | /*================================================================= 2 | * 3 | * planner.c 4 | * 5 | *=================================================================*/ 6 | #include 7 | #include "mex.h" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "rrt_tree.h" 19 | #include "prm_graph.h" 20 | 21 | /* Input Arguments */ 22 | #define MAP_IN prhs[0] 23 | #define ARMSTART_IN prhs[1] 24 | #define ARMGOAL_IN prhs[2] 25 | #define PLANNER_ID_IN prhs[3] 26 | 27 | /* Planner Ids */ 28 | #define RRT 0 29 | #define RRTCONNECT 1 30 | #define RRTSTAR 2 31 | #define PRM 3 32 | 33 | /* Output Arguments */ 34 | #define PLAN_OUT plhs[0] 35 | #define PLANLENGTH_OUT plhs[1] 36 | 37 | #define GETMAPINDEX(X, Y, XSIZE, YSIZE) (Y*XSIZE + X) 38 | 39 | #if !defined(MAX) 40 | #define MAX(A, B) ((A) > (B) ? (A) : (B)) 41 | #endif 42 | 43 | #if !defined(MIN) 44 | #define MIN(A, B) ((A) < (B) ? (A) : (B)) 45 | #endif 46 | 47 | #define PI 3.141592654 48 | 49 | //the length of each link in the arm (should be the same as the one used in runtest.m) 50 | #define LINKLENGTH_CELLS 10 51 | 52 | typedef struct { 53 | int X1, Y1; 54 | int X2, Y2; 55 | int Increment; 56 | int UsingYIndex; 57 | int DeltaX, DeltaY; 58 | int DTerm; 59 | int IncrE, IncrNE; 60 | int XIndex, YIndex; 61 | int Flipped; 62 | } bresenham_param_t; 63 | 64 | 65 | void ContXY2Cell(double x, double y, short unsigned int* pX, short unsigned int *pY, int x_size, int y_size) 66 | { 67 | double cellsize = 1.0; 68 | //take the nearest cell 69 | *pX = (int)(x/(double)(cellsize)); 70 | if( x < 0) *pX = 0; 71 | if( *pX >= x_size) *pX = x_size-1; 72 | 73 | *pY = (int)(y/(double)(cellsize)); 74 | if( y < 0) *pY = 0; 75 | if( *pY >= y_size) *pY = y_size-1; 76 | } 77 | 78 | 79 | void get_bresenham_parameters(int p1x, int p1y, int p2x, int p2y, bresenham_param_t *params) 80 | { 81 | params->UsingYIndex = 0; 82 | 83 | if (fabs((double)(p2y-p1y)/(double)(p2x-p1x)) > 1) 84 | (params->UsingYIndex)++; 85 | 86 | if (params->UsingYIndex) 87 | { 88 | params->Y1=p1x; 89 | params->X1=p1y; 90 | params->Y2=p2x; 91 | params->X2=p2y; 92 | } 93 | else 94 | { 95 | params->X1=p1x; 96 | params->Y1=p1y; 97 | params->X2=p2x; 98 | params->Y2=p2y; 99 | } 100 | 101 | if ((p2x - p1x) * (p2y - p1y) < 0) 102 | { 103 | params->Flipped = 1; 104 | params->Y1 = -params->Y1; 105 | params->Y2 = -params->Y2; 106 | } 107 | else 108 | params->Flipped = 0; 109 | 110 | if (params->X2 > params->X1) 111 | params->Increment = 1; 112 | else 113 | params->Increment = -1; 114 | 115 | params->DeltaX=params->X2-params->X1; 116 | params->DeltaY=params->Y2-params->Y1; 117 | 118 | params->IncrE=2*params->DeltaY*params->Increment; 119 | params->IncrNE=2*(params->DeltaY-params->DeltaX)*params->Increment; 120 | params->DTerm=(2*params->DeltaY-params->DeltaX)*params->Increment; 121 | 122 | params->XIndex = params->X1; 123 | params->YIndex = params->Y1; 124 | } 125 | 126 | void get_current_point(bresenham_param_t *params, int *x, int *y) 127 | { 128 | if (params->UsingYIndex) 129 | { 130 | *y = params->XIndex; 131 | *x = params->YIndex; 132 | if (params->Flipped) 133 | *x = -*x; 134 | } 135 | else 136 | { 137 | *x = params->XIndex; 138 | *y = params->YIndex; 139 | if (params->Flipped) 140 | *y = -*y; 141 | } 142 | } 143 | 144 | int get_next_point(bresenham_param_t *params) 145 | { 146 | if (params->XIndex == params->X2) 147 | { 148 | return 0; 149 | } 150 | params->XIndex += params->Increment; 151 | if (params->DTerm < 0 || (params->Increment < 0 && params->DTerm <= 0)) 152 | params->DTerm += params->IncrE; 153 | else 154 | { 155 | params->DTerm += params->IncrNE; 156 | params->YIndex += params->Increment; 157 | } 158 | return 1; 159 | } 160 | 161 | 162 | 163 | int IsValidLineSegment(double x0, double y0, double x1, double y1, double* map, 164 | int x_size, 165 | int y_size) 166 | 167 | { 168 | bresenham_param_t params; 169 | int nX, nY; 170 | short unsigned int nX0, nY0, nX1, nY1; 171 | 172 | //printf("checking link <%f %f> to <%f %f>\n", x0,y0,x1,y1); 173 | 174 | //make sure the line segment is inside the environment 175 | if(x0 < 0 || x0 >= x_size || 176 | x1 < 0 || x1 >= x_size || 177 | y0 < 0 || y0 >= y_size || 178 | y1 < 0 || y1 >= y_size) 179 | return 0; 180 | 181 | ContXY2Cell(x0, y0, &nX0, &nY0, x_size, y_size); 182 | ContXY2Cell(x1, y1, &nX1, &nY1, x_size, y_size); 183 | 184 | //printf("checking link <%d %d> to <%d %d>\n", nX0,nY0,nX1,nY1); 185 | 186 | //iterate through the points on the segment 187 | get_bresenham_parameters(nX0, nY0, nX1, nY1, ¶ms); 188 | do { 189 | get_current_point(¶ms, &nX, &nY); 190 | if(map[GETMAPINDEX(nX,nY,x_size,y_size)] == 1) 191 | return 0; 192 | } while (get_next_point(¶ms)); 193 | 194 | return 1; 195 | } 196 | 197 | int IsValidArmConfiguration(double* angles, int numofDOFs, double* map, 198 | int x_size, int y_size) 199 | { 200 | double x0,y0,x1,y1; 201 | int i; 202 | 203 | //iterate through all the links starting with the base 204 | x1 = ((double)x_size)/2.0; 205 | y1 = 0; 206 | for(i = 0; i < numofDOFs; i++) 207 | { 208 | //compute the corresponding line segment 209 | x0 = x1; 210 | y0 = y1; 211 | x1 = x0 + LINKLENGTH_CELLS*cos(2*PI-angles[i]); 212 | y1 = y0 - LINKLENGTH_CELLS*sin(2*PI-angles[i]); 213 | 214 | //check the validity of the corresponding line segment 215 | if(!IsValidLineSegment(x0,y0,x1,y1,map,x_size,y_size)) 216 | return 0; 217 | } 218 | return 1; 219 | } 220 | 221 | void incrementToJointAngles(int num_of_dof, double step_size, double** curr_ptr, std::vector& goal) 222 | { 223 | for (int i = 0; i < num_of_dof; i++) { 224 | double temp_dist = goal[i] - (*curr_ptr)[i]; 225 | if (temp_dist > 0.0) { 226 | if (temp_dist < PI) (*curr_ptr)[i] += step_size; 227 | // remember to wrap the angle between 0 and 2pi 228 | else (*curr_ptr)[i] = (((*curr_ptr)[i] - step_size) > 0.0) ? ((*curr_ptr)[i] - step_size) 229 | : (2*PI + (*curr_ptr)[i] - step_size); 230 | } 231 | else { 232 | if (temp_dist > -PI) (*curr_ptr)[i] -= step_size; 233 | // remember to wrap the angle between 0 and 2pi 234 | else (*curr_ptr)[i] = (((*curr_ptr)[i] + step_size) < 2*PI) ? ((*curr_ptr)[i] + step_size) 235 | : ((*curr_ptr)[i] + step_size - 2*PI); 236 | } 237 | //assert((*curr_ptr)[i] > 0.0 && (*curr_ptr)[i] < 2*PI); 238 | } 239 | return; 240 | } 241 | 242 | static void planner( 243 | double* map, 244 | int x_size, 245 | int y_size, 246 | double* armstart_anglesV_rad, 247 | double* armgoal_anglesV_rad, 248 | int numofDOFs, 249 | double*** plan, 250 | int* planlength) 251 | { 252 | //no plan by default 253 | *plan = NULL; 254 | *planlength = 0; 255 | 256 | //for now just do straight interpolation between start and goal checking for the validity of samples 257 | 258 | double distance = 0; 259 | int i,j; 260 | for (j = 0; j < numofDOFs; j++){ 261 | if(distance < fabs(armstart_anglesV_rad[j] - armgoal_anglesV_rad[j])) 262 | distance = fabs(armstart_anglesV_rad[j] - armgoal_anglesV_rad[j]); 263 | } 264 | int numofsamples = (int)(distance/(PI/20)); 265 | if(numofsamples < 2){ 266 | printf("the arm is already at the goal\n"); 267 | return; 268 | } 269 | *plan = (double**) malloc(numofsamples*sizeof(double*)); 270 | int firstinvalidconf = 1; 271 | for (i = 0; i < numofsamples; i++){ 272 | (*plan)[i] = (double*) malloc(numofDOFs*sizeof(double)); 273 | for(j = 0; j < numofDOFs; j++){ 274 | (*plan)[i][j] = armstart_anglesV_rad[j] + ((double)(i)/(numofsamples-1))*(armgoal_anglesV_rad[j] - armstart_anglesV_rad[j]); 275 | } 276 | if(!IsValidArmConfiguration((*plan)[i], numofDOFs, map, x_size, y_size) && firstinvalidconf) 277 | { 278 | firstinvalidconf = 1; 279 | printf("ERROR: Invalid arm configuration!!!\n"); 280 | } 281 | } 282 | *planlength = numofsamples; 283 | 284 | return; 285 | } 286 | 287 | 288 | double calculateCost(std::vector& config1, std::vector& config2) 289 | { 290 | // calcualte L1 cost between two configurations 291 | assert(config1.size() == config2.size()); 292 | double total_cost = 0.0; 293 | for (int i = 0; i < config1.size(); i++) { 294 | total_cost += (fabs(config1[i]-config2[i])>PI) ? (2*PI-fabs(config1[i]-config2[i])) : fabs(config1[i]-config2[i]); 295 | } 296 | return total_cost; 297 | } 298 | 299 | static void plannerRRT(double* map, 300 | int x_size, 301 | int y_size, 302 | double* armstart_anglesV_rad, 303 | double* armgoal_anglesV_rad, 304 | int numofDOFs, 305 | double*** plan, 306 | int* planlength) 307 | { 308 | if (!IsValidArmConfiguration(armstart_anglesV_rad, numofDOFs, map, x_size, y_size) || 309 | !IsValidArmConfiguration(armgoal_anglesV_rad, numofDOFs, map, x_size, y_size)) { 310 | std::cout << "START or GOAL is in collision. Aborted!" << std::endl; 311 | return; 312 | } 313 | 314 | std::uniform_real_distribution goal_bias_generation(0.0, 1.0); 315 | std::uniform_real_distribution joint_ang_generation(0.0, 2*PI); 316 | std::random_device rd; 317 | std::default_random_engine generator(rd()); 318 | 319 | int epsilon = 60; //epsilon here specifies max. num of steps for each iteration 320 | double step_size = PI/90; 321 | 322 | int num_vertices = 0; 323 | 324 | // Create tree object 325 | RRTTree tree(numofDOFs); 326 | 327 | // Add initial pose as vertex 328 | std::vector start_config(armstart_anglesV_rad, armstart_anglesV_rad+numofDOFs); 329 | tree.addVertex(start_config); 330 | tree.setVertexCost(tree.getNodeID()-1, 0.0); 331 | num_vertices++; 332 | 333 | // Create while loop until goal configuration is reached 334 | bool goal_reached = false; 335 | 336 | while (!goal_reached) { 337 | // Initialize new/ sampled configuration 338 | std::vector new_config(numofDOFs); 339 | 340 | // Introduce goal bias here 341 | double bias = goal_bias_generation(generator); 342 | if (bias > 0.80) { 343 | // Set new_config to goal configuration 344 | copy(armgoal_anglesV_rad, armgoal_anglesV_rad+numofDOFs, new_config.begin()); 345 | } 346 | else { 347 | // Sample new arm configuration 348 | //new_config[0] = joint_ang_generation(generator)/2; 349 | for (int i = 0; i < numofDOFs; i++) 350 | new_config[i] = joint_ang_generation(generator); 351 | } 352 | 353 | // Find nearest neighbor 354 | int nn_index = tree.getNearestVertex(new_config); 355 | std::vector nn_config = tree.getNodeConfig(nn_index); 356 | 357 | if (bias < 0.80 && tree.calculateDistance(new_config, nn_config) < 0.10) continue; 358 | 359 | // Attempt to extend to new sample 360 | bool extend = true; 361 | double* temp_config = &nn_config[0]; 362 | double* backup_config = (double*) malloc(numofDOFs*sizeof(double)); 363 | int step_count = 0; 364 | 365 | while (extend) { 366 | // save configuration before increment 367 | copy(temp_config, temp_config+numofDOFs, backup_config); 368 | // increment on each joint angle 369 | incrementToJointAngles(numofDOFs, step_size, &temp_config, new_config); 370 | 371 | // check collision 372 | if (!IsValidArmConfiguration(temp_config, numofDOFs, map, x_size, y_size)) { 373 | copy(backup_config, backup_config+numofDOFs, temp_config); 374 | extend = false; 375 | break; 376 | } 377 | step_count++; 378 | if (step_count > epsilon) extend = false; 379 | 380 | //vector temp_config_vec(temp_config, temp_config+numofDOFs); 381 | //else if (tree.calculateDistance(temp_config_vec, new_config) < 0.1) extend = false; 382 | } 383 | 384 | free(backup_config); 385 | 386 | if (step_count > 0) { 387 | // Update ACUTUAL new configuration 388 | copy(temp_config, temp_config+numofDOFs, new_config.begin()); 389 | // Add new_config to the tree 390 | tree.addVertex(new_config); 391 | tree.addEdge(nn_index, tree.getNodeID()-1); 392 | // Set cost for the new vertex 393 | nn_config = tree.getNodeConfig(nn_index); 394 | double cost_new = tree.getVertexCost(nn_index) + calculateCost(nn_config, new_config); 395 | tree.setVertexCost(tree.getNodeID()-1, cost_new); 396 | 397 | num_vertices++; 398 | if (num_vertices % 1000 == 0) std::cout << "Num of Vertices: " << num_vertices << std::endl; 399 | 400 | double dist_to_goal = 0.0; 401 | for (int i = 0; i < numofDOFs; i++) { 402 | double joint_dist = fabs(new_config[i] - armgoal_anglesV_rad[i]); 403 | dist_to_goal += (joint_dist > PI) ? (2*PI-joint_dist) : joint_dist; 404 | } 405 | if (dist_to_goal < 0.1) goal_reached = true; 406 | } 407 | 408 | if (num_vertices > 50000) { 409 | std::cout << "Aborted due to timeout ... Please re-plan" << std::endl; 410 | break; 411 | } 412 | } 413 | 414 | if (num_vertices <= 50000) { 415 | std::cout << "generating the plan" << std::endl; 416 | //back-track the path and return the plan 417 | std::vector plan_ids = tree.returnPlan(); 418 | int path_length = plan_ids.size(); 419 | 420 | *plan = (double**) malloc(path_length*sizeof(double*)); 421 | for (int i = 0; i < path_length; i++) { 422 | (*plan)[i] = (double*) malloc(numofDOFs*sizeof(double)); 423 | std::vector config = tree.getNodeConfig(plan_ids[path_length-i-1]); 424 | //(*plan)[i] = &config[0]; is WRONG 425 | 426 | // for(int j = 0; j < numofDOFs; j++) std::cout << config[j] << " "; 427 | // std::cout << std::endl; 428 | copy(config.begin(), config.end(), (*plan)[i]); 429 | } 430 | std::cout << "Cost to goal: " << tree.getVertexCost(tree.getNodeID()-1) << std::endl; 431 | std::cout << "Number of vertices: " << num_vertices << std::endl; 432 | *planlength = path_length; 433 | } 434 | 435 | return; 436 | } 437 | 438 | 439 | static void plannerRRTConnect(double* map, 440 | int x_size, 441 | int y_size, 442 | double* armstart_anglesV_rad, 443 | double* armgoal_anglesV_rad, 444 | int numofDOFs, 445 | double*** plan, 446 | int* planlength) 447 | { 448 | if (!IsValidArmConfiguration(armstart_anglesV_rad, numofDOFs, map, x_size, y_size) || 449 | !IsValidArmConfiguration(armgoal_anglesV_rad, numofDOFs, map, x_size, y_size)) { 450 | std::cout << "START or GOAL is in collision. Aborted!" << std::endl; 451 | return; 452 | } 453 | 454 | // Generate sampling tool 455 | std::uniform_real_distribution goal_bias_generation(0.0, 1.0); 456 | std::uniform_real_distribution joint_ang_generation(0.0, 2*PI); 457 | std::random_device rd; 458 | std::default_random_engine generator(rd()); 459 | 460 | int epsilon = 30; //epsilon here specifies max. num of steps for each iteration 461 | double step_size = PI/180; 462 | 463 | int num_vertices = 0; 464 | 465 | // Create tree objects 466 | RRTTree tree_s(numofDOFs); 467 | RRTTree tree_g(numofDOFs); 468 | 469 | // Add initial pose as vertex 470 | std::vector start_config(armstart_anglesV_rad, armstart_anglesV_rad+numofDOFs); 471 | std::vector goal_config(armgoal_anglesV_rad, armgoal_anglesV_rad+numofDOFs); 472 | tree_s.addVertex(start_config); 473 | tree_g.addVertex(goal_config); 474 | num_vertices += 2; 475 | 476 | // Initialize new / sampled configurations 477 | std::vector new_config(numofDOFs); 478 | std::vector rand_config(numofDOFs); 479 | // Initialize vector to store nearest neighbor's configuration 480 | std::vector nn_config(numofDOFs); 481 | int nn_index = -1; 482 | 483 | // Create while loop until the two trees are connected 484 | bool connected = false; 485 | int num_iterations = 0; 486 | 487 | while (!connected) { 488 | // if (num_iterations % 2 == 0)->Extend tree_s to q_rand; Extend tree_g to q_new 489 | // if (num_iterations % 2 == 1)->Extend tree_g to q_rand; Extend tree_s to q_new 490 | 491 | // Introduce goal bias here 492 | double bias = goal_bias_generation(generator); 493 | if (bias > 0.90) { 494 | // Set rand_config to goal configuration 495 | if (num_iterations % 2 == 0) 496 | copy(armgoal_anglesV_rad, armgoal_anglesV_rad+numofDOFs, rand_config.begin()); 497 | else 498 | copy(armstart_anglesV_rad, armstart_anglesV_rad+numofDOFs, rand_config.begin()); 499 | } 500 | else { 501 | // Sample new arm configuration 502 | for (int i = 0; i < numofDOFs; i++) 503 | rand_config[i] = joint_ang_generation(generator); 504 | } 505 | // Find nearest neighbor 506 | if (num_iterations % 2 == 0) { 507 | nn_index = tree_s.getNearestVertex(rand_config); 508 | nn_config = tree_s.getNodeConfig(nn_index); 509 | } else { 510 | nn_index = tree_g.getNearestVertex(rand_config); 511 | nn_config = tree_g.getNodeConfig(nn_index); 512 | } 513 | 514 | // Ignore sample that is too close to existing vertices 515 | if (bias < 0.90 && tree_s.calculateDistance(rand_config, nn_config) < 0.20) continue; 516 | 517 | // Attempt to extend to new sample 518 | bool extend = true; 519 | double* temp_config = &nn_config[0]; 520 | double* backup_config = (double*) malloc(numofDOFs*sizeof(double)); 521 | int step_count = 0; 522 | 523 | while (extend) { 524 | // save configuration before increment 525 | copy(temp_config, temp_config+numofDOFs, backup_config); 526 | // increment on each joint angle 527 | incrementToJointAngles(numofDOFs, step_size, &temp_config, rand_config); 528 | 529 | // check collision 530 | if (!IsValidArmConfiguration(temp_config, numofDOFs, map, x_size, y_size)) { 531 | copy(backup_config, backup_config+numofDOFs, temp_config); 532 | break; 533 | } 534 | step_count++; 535 | if (step_count > epsilon) extend = false; 536 | } 537 | 538 | // If not trapped... 539 | if (step_count > 0) { 540 | // Update ACUTUAL new configuration 541 | copy(temp_config, temp_config+numofDOFs, new_config.begin()); 542 | 543 | if (num_iterations % 2 == 0) { 544 | // Add new_config to the tree 545 | tree_s.addVertex(new_config); 546 | tree_s.addEdge(nn_index, tree_s.getNodeID()-1); 547 | 548 | // Find nearest neighbor of new_config from tree_g 549 | nn_index = tree_g.getNearestVertex(new_config); 550 | nn_config = tree_g.getNodeConfig(nn_index); 551 | 552 | } else { 553 | // Add new_config to the tree 554 | tree_g.addVertex(new_config); 555 | tree_g.addEdge(nn_index, tree_g.getNodeID()-1); 556 | // Find nearest neighbor of new_config from tree_s 557 | nn_index = tree_s.getNearestVertex(new_config); 558 | nn_config = tree_s.getNodeConfig(nn_index); 559 | } 560 | num_vertices++; 561 | if (num_vertices % 100 == 0) std::cout << "Num of Vertices: " << num_vertices << std::endl; 562 | 563 | // Attempt to extend to new sample 564 | // Reset parameters 565 | extend = true; 566 | temp_config = &nn_config[0]; 567 | step_count = 0; 568 | 569 | while (extend) { 570 | // save configuration before increment 571 | copy(temp_config, temp_config+numofDOFs, backup_config); 572 | // increment on each joint angle 573 | incrementToJointAngles(numofDOFs, step_size, &temp_config, new_config); 574 | 575 | step_count++; 576 | if (step_count > epsilon) extend = false; 577 | 578 | // check collision 579 | if (!IsValidArmConfiguration(temp_config, numofDOFs, map, x_size, y_size)) { 580 | copy(backup_config, backup_config+numofDOFs, temp_config); 581 | extend = false; 582 | } 583 | // check if two trees are connected 584 | double dist_to_new = 0.0; 585 | for (int i = 0; i < numofDOFs; i++) { 586 | double joint_dist = fabs(new_config[i] - temp_config[i]); 587 | dist_to_new += (joint_dist > PI) ? (2*PI-joint_dist) : joint_dist; 588 | } 589 | if (dist_to_new < 0.1) { 590 | connected = true; 591 | extend = false; 592 | } 593 | } 594 | 595 | if (step_count > 0) { 596 | // Update ACUTUAL new configuration 597 | if (!connected) copy(temp_config, temp_config+numofDOFs, new_config.begin()); 598 | 599 | // Add new_config to the tree 600 | if (num_iterations % 2 == 0) { 601 | tree_g.addVertex(new_config); 602 | tree_g.addEdge(nn_index, tree_g.getNodeID()-1); 603 | } else { 604 | tree_s.addVertex(new_config); 605 | tree_s.addEdge(nn_index, tree_s.getNodeID()-1); 606 | } 607 | num_vertices++; 608 | if (num_vertices % 100 == 0) std::cout << "Num of Vertices: " << num_vertices << std::endl; 609 | } 610 | } 611 | // remember to free backup pointer 612 | free(backup_config); 613 | 614 | num_iterations++; 615 | } 616 | 617 | std::cout << "generating the plan" << std::endl; 618 | //back-track the path and return the plan 619 | std::vector plan1_ids; 620 | std::vector plan2_ids; 621 | if (num_iterations % 2 == 0) { 622 | plan1_ids = tree_s.returnPlan(nn_index); 623 | plan2_ids = tree_g.returnPlan(); 624 | } else { 625 | plan1_ids = tree_s.returnPlan(); 626 | plan2_ids = tree_g.returnPlan(nn_index); 627 | } 628 | int path_length = plan1_ids.size() + plan2_ids.size(); 629 | 630 | double total_cost = 0.0; 631 | std::vector config; 632 | std::vector prev_config; 633 | *plan = (double**) malloc(path_length*sizeof(double*)); 634 | for (int i = 0; i < plan1_ids.size(); i++) { 635 | (*plan)[i] = (double*) malloc(numofDOFs*sizeof(double)); 636 | config = tree_s.getNodeConfig(plan1_ids[plan1_ids.size()-i-1]); 637 | copy(config.begin(), config.end(), (*plan)[i]); 638 | 639 | if (i > 0) total_cost += calculateCost(prev_config, config); 640 | prev_config = config; 641 | } 642 | prev_config = tree_g.getNodeConfig(plan1_ids[0]); 643 | for (int i = 0; i < plan2_ids.size(); i++) { 644 | (*plan)[i+plan1_ids.size()] = (double*) malloc(numofDOFs*sizeof(double)); 645 | config = tree_g.getNodeConfig(plan2_ids[i]); 646 | copy(config.begin(), config.end(), (*plan)[i+plan1_ids.size()]); 647 | 648 | total_cost += calculateCost(prev_config, config); 649 | prev_config = config; 650 | } 651 | std::cout << "Cost to goal: " << total_cost << std::endl; 652 | std::cout << "Number of vertices: " << num_vertices << std::endl; 653 | *planlength = path_length; 654 | 655 | return; 656 | } 657 | 658 | bool isPathValid(std::vector new_config, std::vector& neighbor_config, 659 | int numofDOFs, double* map, int x_size, int y_size) 660 | { 661 | double distance = 0; 662 | for (int i = 0; i < numofDOFs; i++){ 663 | if(distance < fabs(new_config[i] - neighbor_config[i])) 664 | distance = fabs(new_config[i] - neighbor_config[i]); 665 | } 666 | int numofsamples = (int)(distance/(PI/90)); 667 | if (numofsamples < 2) return true; 668 | 669 | double* temp_config = &new_config[0]; 670 | for (int i = 0; i < numofsamples; i++) { 671 | for (int i = 0; i < numofDOFs; i++) { 672 | temp_config[i] += (neighbor_config[i] - new_config[i])/(double)numofsamples; 673 | } 674 | if (!IsValidArmConfiguration(temp_config, numofDOFs, map, x_size, y_size)) return false; 675 | } 676 | return true; 677 | } 678 | 679 | static void plannerRRTStar(double* map, 680 | int x_size, 681 | int y_size, 682 | double* armstart_anglesV_rad, 683 | double* armgoal_anglesV_rad, 684 | int numofDOFs, 685 | double*** plan, 686 | int* planlength) 687 | { 688 | if (!IsValidArmConfiguration(armstart_anglesV_rad, numofDOFs, map, x_size, y_size) || 689 | !IsValidArmConfiguration(armgoal_anglesV_rad, numofDOFs, map, x_size, y_size)) { 690 | std::cout << "START or GOAL is in collision. Aborted!" << std::endl; 691 | return; 692 | } 693 | 694 | std::uniform_real_distribution goal_bias_generation(0.0, 1.0); 695 | std::uniform_real_distribution joint_ang_generation(0.0, 2*PI); 696 | std::random_device rd; 697 | std::default_random_engine generator(rd()); 698 | 699 | int epsilon = 60; //epsilon here specifies max. num of steps for each iteration 700 | double step_size = PI/90; 701 | 702 | int num_vertices = 0; 703 | 704 | // Create tree object 705 | RRTTree tree(numofDOFs); 706 | 707 | // Add initial pose as vertex 708 | std::vector start_config(armstart_anglesV_rad, armstart_anglesV_rad+numofDOFs); 709 | tree.addVertex(start_config); 710 | tree.setVertexCost(tree.getNodeID()-1, 0.0); 711 | num_vertices++; 712 | 713 | // Create while loop until goal configuration is reached 714 | bool goal_reached = false; 715 | 716 | while (!goal_reached) { 717 | // Initialize new/ sampled configuration 718 | std::vector new_config(numofDOFs); 719 | 720 | // Introduce goal bias here 721 | double bias = goal_bias_generation(generator); 722 | if (bias > 0.80) { 723 | // Set new_config to goal configuration 724 | copy(armgoal_anglesV_rad, armgoal_anglesV_rad+numofDOFs, new_config.begin()); 725 | } 726 | else { 727 | // Sample new arm configuration 728 | for (int i = 0; i < numofDOFs; i++) 729 | new_config[i] = joint_ang_generation(generator); 730 | } 731 | 732 | // Find nearest neighbor 733 | int nn_index = tree.getNearestVertex(new_config); 734 | std::vector nn_config = tree.getNodeConfig(nn_index); 735 | 736 | if (bias < 0.80 && tree.calculateDistance(new_config, nn_config) < 0.1) continue; 737 | 738 | // Attempt to extend to new sample 739 | bool extend = true; 740 | double* temp_config = &nn_config[0]; 741 | double* backup_config = (double*) malloc(numofDOFs*sizeof(double)); 742 | int step_count = 0; 743 | 744 | while (extend) { 745 | // save configuration before increment 746 | copy(temp_config, temp_config+numofDOFs, backup_config); 747 | // increment on each joint angle 748 | incrementToJointAngles(numofDOFs, step_size, &temp_config, new_config); 749 | 750 | // check collision 751 | if (!IsValidArmConfiguration(temp_config, numofDOFs, map, x_size, y_size)) { 752 | copy(backup_config, backup_config+numofDOFs, temp_config); 753 | extend = false; 754 | } 755 | else { 756 | step_count++; 757 | if (step_count > epsilon) extend = false; 758 | } 759 | } 760 | 761 | free(backup_config); 762 | 763 | if (step_count > 0) { 764 | // Update ACUTUAL new configuration 765 | copy(temp_config, temp_config+numofDOFs, new_config.begin()); 766 | // Add new_config to the tree 767 | tree.addVertex(new_config); 768 | num_vertices++; 769 | 770 | if (num_vertices % 1000 == 0) std::cout << "Num of Vertices: " << num_vertices << std::endl; 771 | 772 | // Add cost of new vertex 773 | nn_config = tree.getNodeConfig(nn_index); 774 | double cost_new = tree.getVertexCost(nn_index) + calculateCost(nn_config, new_config); 775 | tree.setVertexCost(tree.getNodeID()-1, cost_new); 776 | 777 | // Find all neighbors within certain distance from new_config 778 | double radius = 1.6*pow(log(num_vertices)/num_vertices, 1/numofDOFs); //epsilon * step_size; 779 | std::vector near_neighbors = tree.getNearVertices(nn_index, radius); 780 | 781 | //if (num_vertices % 1000 == 0) std::cout << "Num of near neighbors: " << near_neighbors.size() << std::endl; 782 | 783 | // Try to find best parent for new config 784 | // (Is there better path to get ot new vertex from existing neighbors) 785 | std::vector neighbor_config; 786 | int min_neighbor_id = nn_index; 787 | // record indices of valid neighbors of new config (i.e. collision-free) 788 | std::vector valid_neighbor_id; 789 | 790 | for (int i = 0; i < near_neighbors.size(); i++) { 791 | neighbor_config = tree.getNodeConfig(near_neighbors[i]); 792 | if (isPathValid(new_config, neighbor_config, numofDOFs, map, x_size, y_size)) { 793 | double cost_temp = tree.getVertexCost(near_neighbors[i]) + calculateCost(new_config, neighbor_config); 794 | if (cost_temp + 0.1 < cost_new) { 795 | // record index of x_min 796 | min_neighbor_id = near_neighbors[i]; 797 | // update cost of new vertex 798 | cost_new = cost_temp; 799 | tree.setVertexCost(tree.getNodeID()-1, cost_new); 800 | } 801 | valid_neighbor_id.push_back(near_neighbors[i]); 802 | } 803 | } 804 | 805 | // Add edge for new vertex now 806 | tree.addEdge(min_neighbor_id, tree.getNodeID()-1); 807 | 808 | // Try to update cost of each vertex 809 | // (Is there better path (through new vertex) to some existing neighbor w/lower cost) 810 | for (int i = 0; i < valid_neighbor_id.size(); i++) { 811 | if (valid_neighbor_id[i] != min_neighbor_id) { 812 | neighbor_config = tree.getNodeConfig(valid_neighbor_id[i]); 813 | double cost_temp = cost_new + calculateCost(new_config, neighbor_config); 814 | // In case cost(near) > cost(new) + cost(new,near) 815 | // If so, disconnect existing node and its parent AND add new edges 816 | if (cost_temp + 0.1 < tree.getVertexCost(valid_neighbor_id[i])) { 817 | // update neighbor's cost 818 | tree.setVertexCost(valid_neighbor_id[i], cost_temp); 819 | // remove connection (edge) between the neighbor and its parent 820 | tree.removeEdge(valid_neighbor_id[i]); 821 | // add connection (edge) between the neighbor and new vertex 822 | tree.addEdge(tree.getNodeID()-1, valid_neighbor_id[i]); 823 | } 824 | } 825 | } 826 | // Check whether we reach the goal 827 | double dist_to_goal = 0.0; 828 | for (int i = 0; i < numofDOFs; i++) { 829 | double joint_dist = fabs(new_config[i] - armgoal_anglesV_rad[i]); 830 | dist_to_goal += (joint_dist > PI) ? (2*PI-joint_dist) : joint_dist; 831 | } 832 | if (dist_to_goal < 0.1) goal_reached = true; 833 | } 834 | if (num_vertices > 50000) { 835 | std::cout << "Aborted due to timeout ... Please re-plan" << std::endl; 836 | break; 837 | } 838 | } 839 | 840 | if (num_vertices <= 50000) { 841 | std::cout << "generating the plan" << std::endl; 842 | //back-track the path and return the plan 843 | std::vector plan_ids = tree.returnPlan(); 844 | int path_length = plan_ids.size(); 845 | 846 | *plan = (double**) malloc(path_length*sizeof(double*)); 847 | for (int i = 0; i < path_length; i++) { 848 | (*plan)[i] = (double*) malloc(numofDOFs*sizeof(double)); 849 | std::vector config = tree.getNodeConfig(plan_ids[path_length-i-1]); 850 | copy(config.begin(), config.end(), (*plan)[i]); 851 | } 852 | std::cout << "Cost to goal: " << tree.getVertexCost(tree.getNodeID()-1) << std::endl; 853 | std::cout << "Number of vertices: " << num_vertices << std::endl; 854 | *planlength = path_length; 855 | } 856 | 857 | return; 858 | } 859 | 860 | static void plannerPRM(double* map, 861 | int x_size, 862 | int y_size, 863 | double* armstart_anglesV_rad, 864 | double* armgoal_anglesV_rad, 865 | int numofDOFs, 866 | double*** plan, 867 | int* planlength) 868 | { 869 | /********************Building Roadmap************************/ 870 | std::uniform_real_distribution joint_ang_generation(0.0, 2*PI); 871 | std::random_device rd; 872 | std::default_random_engine generator(rd()); 873 | 874 | // Create graph object 875 | PRMGraph graph(numofDOFs); 876 | 877 | // K specifies number of neighbors we try to connect with new vertex 878 | int K = 10; 879 | int num_vertices = 0; 880 | 881 | bool extend = true; 882 | double* new_config = (double*) malloc(numofDOFs*sizeof(double)); 883 | 884 | while (extend) { 885 | // Random sampling for new vertex/configuration 886 | for (int i = 0; i < numofDOFs; i++) new_config[i] = joint_ang_generation(generator); 887 | 888 | // Check if new vertex/configuration is valid (collision-free) 889 | if (IsValidArmConfiguration(new_config, numofDOFs, map, x_size, y_size)) { 890 | // Add new vertex to graph 891 | std::vector new_vertex(5,0.0); 892 | copy(new_config, new_config+numofDOFs, new_vertex.begin()); 893 | graph.addVertex(new_vertex); 894 | num_vertices++; 895 | 896 | if (num_vertices > 100) { 897 | // Find neighbors of new vertex in the graph 898 | std::vector knn_id = graph.findKNN(new_vertex, K); 899 | 900 | // Check if the path from new vertex to the neighbor is valid (collision-free) 901 | std::vector neighbor_vertex; 902 | for (int i = 0; i < knn_id.size(); i++) { 903 | neighbor_vertex = graph.getNodeConfig(knn_id[i]); 904 | // If valid, add edge between new vertex and the neighbor in the graph 905 | if (isPathValid(new_vertex, neighbor_vertex, numofDOFs, map, x_size, y_size)) 906 | graph.addEdge(knn_id[i], graph.getCurrentNodeID()-1); 907 | } 908 | } 909 | } 910 | //if (num_vertices%100 == 0) std::cout << num_vertices << std::endl; 911 | if (num_vertices >= 5000) extend = false; 912 | } 913 | 914 | free(new_config); 915 | 916 | std::cout << "Roadmap construction is completed. Searching for path now." << std::endl; 917 | 918 | /********************Searching Path*************************/ 919 | 920 | std::vector start_config(armstart_anglesV_rad, armstart_anglesV_rad + numofDOFs); 921 | std::vector goal_config(armgoal_anglesV_rad, armgoal_anglesV_rad + numofDOFs); 922 | 923 | int start_on_graph_id = graph.getNearestVertex(start_config); 924 | int goal_on_graph_id = graph.getNearestVertex(goal_config); 925 | 926 | std::vector start_on_graph = graph.getNodeConfig(start_on_graph_id); 927 | std::vector goal_on_graph = graph.getNodeConfig(goal_on_graph_id); 928 | 929 | if (!isPathValid(start_config, start_on_graph, numofDOFs, map, x_size, y_size)) { 930 | std::cout << "Cannot connect START to the graph...Please re-plan." << std::endl; 931 | return; 932 | } 933 | 934 | if (!isPathValid(goal_config, goal_on_graph, numofDOFs, map, x_size, y_size)) { 935 | std::cout << "Cannot connect GOAL to the graph...Please re-plan." << std::endl; 936 | return; 937 | } 938 | 939 | // Perform Depth-First Search 940 | /******************************************************************** 941 | std::stack DFS_stack; 942 | DFS_stack.push(start_on_graph_id); 943 | 944 | bool goal_reached = false; 945 | if (start_on_graph_id == goal_on_graph_id) goal_reached = true; 946 | 947 | // Create dictionary with key = node_id (visited) and value = parent_id 948 | std::unordered_map node_info; 949 | 950 | int curr_id; 951 | while (!goal_reached && !DFS_stack.empty()) { 952 | // Get the vertex at the top of stack 953 | curr_id = DFS_stack.top(); 954 | DFS_stack.pop(); 955 | 956 | vector curr_neighbors = graph.getNeighborsID(curr_id); 957 | // Add neighbors to the stack 958 | for (int i = 0; i < curr_neighbors.size(); i++) { 959 | if (curr_neighbors[i] == goal_on_graph_id) { 960 | node_info[goal_on_graph_id] = curr_id; 961 | goal_reached = true; 962 | break; 963 | } 964 | else if (node_info.find(curr_neighbors[i]) == node_info.end()) { 965 | node_info[curr_neighbors[i]] = curr_id; 966 | DFS_stack.push(curr_neighbors[i]); 967 | } 968 | } 969 | } 970 | ********************************************************************/ 971 | 972 | // Perform Breath-First Search 973 | std::queue BFS_queue; 974 | BFS_queue.push(start_on_graph_id); 975 | 976 | bool goal_reached = false; 977 | if (start_on_graph_id == goal_on_graph_id) goal_reached = true; 978 | 979 | // Create dictionary with key = node_id (visited) and value = parent_id 980 | std::unordered_map node_info; 981 | 982 | int curr_id; 983 | while (!goal_reached && !BFS_queue.empty()) { 984 | // Get the vertex at the top of stack 985 | curr_id = BFS_queue.front(); 986 | BFS_queue.pop(); 987 | 988 | vector curr_neighbors = graph.getNeighborsID(curr_id); 989 | // Add neighbors to the stack 990 | for (int i = 0; i < curr_neighbors.size(); i++) { 991 | if (curr_neighbors[i] == goal_on_graph_id) { 992 | node_info[goal_on_graph_id] = curr_id; 993 | goal_reached = true; 994 | break; 995 | } 996 | else if (node_info.find(curr_neighbors[i]) == node_info.end()) { 997 | node_info[curr_neighbors[i]] = curr_id; 998 | BFS_queue.push(curr_neighbors[i]); 999 | } 1000 | } 1001 | } 1002 | 1003 | 1004 | 1005 | if (goal_reached) { 1006 | std::vector plan_ids; 1007 | int index_iter = goal_on_graph_id; 1008 | double total_cost = 0.0; 1009 | 1010 | while (index_iter != start_on_graph_id) { 1011 | plan_ids.push_back(index_iter); 1012 | index_iter = node_info[index_iter]; 1013 | } 1014 | plan_ids.push_back(start_on_graph_id); 1015 | 1016 | int path_length = plan_ids.size()+2; 1017 | *plan = (double**) malloc(path_length*sizeof(double*)); 1018 | 1019 | (*plan)[0] = (double*) malloc(numofDOFs*sizeof(double)); 1020 | copy(armstart_anglesV_rad, armstart_anglesV_rad + numofDOFs, (*plan)[0]); 1021 | 1022 | std::vector config; 1023 | std::vector prev_config = start_config; 1024 | 1025 | for (int i = 0; i < plan_ids.size(); i++) { 1026 | (*plan)[i+1] = (double*) malloc(numofDOFs*sizeof(double)); 1027 | config = graph.getNodeConfig(plan_ids[plan_ids.size()-i-1]); 1028 | copy(config.begin(), config.end(), (*plan)[i+1]); 1029 | 1030 | if (i > 0) total_cost += calculateCost(prev_config, config); 1031 | prev_config = config; 1032 | } 1033 | 1034 | (*plan)[path_length-1] = (double*) malloc(numofDOFs*sizeof(double)); 1035 | copy(armgoal_anglesV_rad, armgoal_anglesV_rad + numofDOFs, (*plan)[path_length-1]); 1036 | 1037 | total_cost += calculateCost(prev_config, goal_config); 1038 | 1039 | std::cout << "Cost to goal: " << total_cost << std::endl; 1040 | *planlength = path_length; 1041 | } 1042 | else { 1043 | std::cout << "Unable to connect START and GOAL on exisiting roadmap" << std::endl; 1044 | } 1045 | 1046 | 1047 | /**************For visualizing vertices only***************/ 1048 | 1049 | // *plan = (double**) malloc(num_vertices*sizeof(double*)); 1050 | 1051 | // for (int i = 0; i < num_vertices; i++) { 1052 | // (*plan)[i] = (double*) malloc(numofDOFs*sizeof(double)); 1053 | // std::vector config = graph.getNodeConfig(i); 1054 | // copy(config.begin(), config.end(), (*plan)[i]); 1055 | // for(int j = 0; j < numofDOFs; j++) std::cout << (*plan)[i][j] << " "; 1056 | // std::cout << std::endl; 1057 | // } 1058 | // *planlength = num_vertices; 1059 | /*********************************************************/ 1060 | 1061 | return; 1062 | } 1063 | 1064 | //prhs contains input parameters (3): 1065 | //1st is matrix with all the obstacles 1066 | //2nd is a row vector of start angles for the arm 1067 | //3nd is a row vector of goal angles for the arm 1068 | //plhs should contain output parameters (2): 1069 | //1st is a 2D matrix plan when each plan[i][j] is the value of jth angle at the ith step of the plan 1070 | //(there are D DoF of the arm (that is, D angles). So, j can take values from 0 to D-1 1071 | //2nd is planlength (int) 1072 | void mexFunction( int nlhs, mxArray *plhs[], 1073 | int nrhs, const mxArray*prhs[]) 1074 | 1075 | { 1076 | 1077 | /* Check for proper number of arguments */ 1078 | if (nrhs != 4) { 1079 | mexErrMsgIdAndTxt( "MATLAB:planner:invalidNumInputs", 1080 | "Four input arguments required."); 1081 | } else if (nlhs != 2) { 1082 | mexErrMsgIdAndTxt( "MATLAB:planner:maxlhs", 1083 | "One output argument required."); 1084 | } 1085 | 1086 | /* get the dimensions of the map and the map matrix itself*/ 1087 | int x_size = (int) mxGetM(MAP_IN); 1088 | int y_size = (int) mxGetN(MAP_IN); 1089 | double* map = mxGetPr(MAP_IN); 1090 | 1091 | /* get the start and goal angles*/ 1092 | int numofDOFs = (int) (MAX(mxGetM(ARMSTART_IN), mxGetN(ARMSTART_IN))); 1093 | if(numofDOFs <= 1){ 1094 | mexErrMsgIdAndTxt( "MATLAB:planner:invalidnumofdofs", 1095 | "it should be at least 2"); 1096 | } 1097 | double* armstart_anglesV_rad = mxGetPr(ARMSTART_IN); 1098 | if (numofDOFs != MAX(mxGetM(ARMGOAL_IN), mxGetN(ARMGOAL_IN))){ 1099 | mexErrMsgIdAndTxt( "MATLAB:planner:invalidnumofdofs", 1100 | "numofDOFs in startangles is different from goalangles"); 1101 | } 1102 | double* armgoal_anglesV_rad = mxGetPr(ARMGOAL_IN); 1103 | 1104 | //get the planner id 1105 | int planner_id = (int)*mxGetPr(PLANNER_ID_IN); 1106 | if(planner_id < 0 || planner_id > 3){ 1107 | mexErrMsgIdAndTxt( "MATLAB:planner:invalidplanner_id", 1108 | "planner id should be between 0 and 3 inclusive"); 1109 | } 1110 | 1111 | //call the planner 1112 | double** plan = NULL; 1113 | int planlength = 0; 1114 | 1115 | //you can may be call the corresponding planner function here 1116 | if (planner_id == RRT) 1117 | { 1118 | clock_t tStart = clock(); 1119 | plannerRRT(map,x_size,y_size, armstart_anglesV_rad, armgoal_anglesV_rad, numofDOFs, &plan, &planlength); 1120 | std::cout << "Time taken for planning : " << (double)(clock() - tStart)/CLOCKS_PER_SEC << " seconds" << std::endl; 1121 | } 1122 | else if (planner_id == RRTCONNECT) 1123 | { 1124 | clock_t tStart = clock(); 1125 | plannerRRTConnect(map,x_size,y_size, armstart_anglesV_rad, armgoal_anglesV_rad, numofDOFs, &plan, &planlength); 1126 | std::cout << "Time taken for planning : " << (double)(clock() - tStart)/CLOCKS_PER_SEC << " seconds" << std::endl; 1127 | } 1128 | else if (planner_id == RRTSTAR) 1129 | { 1130 | clock_t tStart = clock(); 1131 | plannerRRTStar(map,x_size,y_size, armstart_anglesV_rad, armgoal_anglesV_rad, numofDOFs, &plan, &planlength); 1132 | std::cout << "Time taken for planning : " << (double)(clock() - tStart)/CLOCKS_PER_SEC << " seconds" << std::endl; 1133 | } 1134 | else if (planner_id == PRM) 1135 | { 1136 | clock_t tStart = clock(); 1137 | plannerPRM(map,x_size,y_size, armstart_anglesV_rad, armgoal_anglesV_rad, numofDOFs, &plan, &planlength); 1138 | std::cout << "Time taken for planning : " << (double)(clock() - tStart)/CLOCKS_PER_SEC << " seconds" << std::endl; 1139 | } 1140 | else { 1141 | //dummy planner which only computes interpolated path 1142 | planner(map,x_size,y_size, armstart_anglesV_rad, armgoal_anglesV_rad, numofDOFs, &plan, &planlength); 1143 | } 1144 | 1145 | printf("planner returned plan of length=%d\n", planlength); 1146 | 1147 | /* Create return values */ 1148 | if(planlength > 0) 1149 | { 1150 | PLAN_OUT = mxCreateNumericMatrix( (mwSize)planlength, (mwSize)numofDOFs, mxDOUBLE_CLASS, mxREAL); 1151 | double* plan_out = mxGetPr(PLAN_OUT); 1152 | //copy the values 1153 | int i,j; 1154 | for(i = 0; i < planlength; i++) 1155 | { 1156 | for (j = 0; j < numofDOFs; j++) 1157 | { 1158 | plan_out[j*planlength + i] = plan[i][j]; 1159 | } 1160 | free(plan[i]); 1161 | } 1162 | free(plan); 1163 | } 1164 | else 1165 | { 1166 | PLAN_OUT = mxCreateNumericMatrix( (mwSize)1, (mwSize)numofDOFs, mxDOUBLE_CLASS, mxREAL); 1167 | double* plan_out = mxGetPr(PLAN_OUT); 1168 | //copy the values 1169 | int j; 1170 | for(j = 0; j < numofDOFs; j++) 1171 | { 1172 | plan_out[j] = armstart_anglesV_rad[j]; 1173 | } 1174 | } 1175 | PLANLENGTH_OUT = mxCreateNumericMatrix( (mwSize)1, (mwSize)1, mxINT8_CLASS, mxREAL); 1176 | double* planlength_out = mxGetPr(PLANLENGTH_OUT); 1177 | //char* planlength_out = (char*)mxGetData(PLANLENGTH_OUT); 1178 | *planlength_out = planlength; 1179 | 1180 | 1181 | return; 1182 | 1183 | } 1184 | 1185 | 1186 | 1187 | 1188 | 1189 | -------------------------------------------------------------------------------- /planner.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhzhang0225/sampling_based_planners/4ec94be299379661eb9933e399b193fd727423bd/planner.mexa64 -------------------------------------------------------------------------------- /prm_graph.cpp: -------------------------------------------------------------------------------- 1 | #include "prm_graph.h" 2 | 3 | PRMGraph::PRMGraph(int numofDOFs) 4 | : num_dof_(numofDOFs), 5 | node_id_(0) { } 6 | 7 | int PRMGraph::getCurrentNodeID() 8 | { 9 | return this->node_id_; 10 | } 11 | 12 | void PRMGraph::addVertex(vector& vertex) 13 | { 14 | // add one row 15 | (this->vertices_).push_back(vector()); 16 | // fill in the new row 17 | vertices_[this->node_id_] = vertex; 18 | 19 | // add place-holder 20 | (this->edges_).push_back(vector()); 21 | 22 | (this->node_id_)++; 23 | } 24 | 25 | vector PRMGraph::getNodeConfig(int node_id) 26 | { 27 | return (this->vertices_)[node_id]; 28 | } 29 | 30 | vector PRMGraph::getNeighborsID(int node_id) 31 | { 32 | return (this->edges_)[node_id]; 33 | } 34 | 35 | void PRMGraph::addEdge(int vertex1_id, int vertex2_id) 36 | { 37 | (this->edges_)[vertex1_id].push_back(vertex2_id); 38 | (this->edges_)[vertex2_id].push_back(vertex1_id); 39 | //std::cout << (this->edges_)[vertex1_id].size() << " " << (this->edges_)[vertex2_id].size() << std::endl; 40 | } 41 | 42 | vector PRMGraph::findKNN(vector& new_vertex, int k) 43 | { 44 | vector> dist; 45 | vector knn_id; 46 | // perform linear search to get K nearest neighbors 47 | 48 | // calculate distance for all vertices 49 | for (int i=0; i<(this->vertices_).size(); i++) { 50 | auto curr_dist = make_pair(i, this->calculateDistance(new_vertex, (this->vertices_)[i])); 51 | dist.push_back(curr_dist); 52 | } 53 | // sort the distance 54 | sort(dist.begin(), dist.end(), 55 | [](const pair& p1, const pair& p2) 56 | { 57 | return (p1.second < p2.second); 58 | }); 59 | 60 | // find id of k nearest neighbors 61 | for (int i=0; i 0.1) knn_id.push_back(dist[i].first); 63 | if (knn_id.size() >= k) break; 64 | } 65 | return knn_id; 66 | } 67 | 68 | int PRMGraph::getNearestVertex(vector& new_vertex) 69 | { 70 | double min_dist = DBL_MAX; 71 | int min_index = -1; 72 | double curr_dist = 0.0; 73 | // perform linear search to get the nearest neighbor 74 | for (int i=0; i<(this->vertices_).size(); i++) { 75 | curr_dist = this->calculateDistance(new_vertex, (this->vertices_)[i]); 76 | if (curr_dist < min_dist) { 77 | min_dist = curr_dist; 78 | min_index = i; 79 | } 80 | } 81 | return min_index; 82 | } -------------------------------------------------------------------------------- /prm_graph.h: -------------------------------------------------------------------------------- 1 | #ifndef PRMGRAPH_H 2 | #define PRMGRAPH_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | /**************************************************** 12 | Class for creating and maintaining a graph structure 13 | as the representation of Probabilistic Roadmap 14 | ****************************************************/ 15 | 16 | #define PI 3.141592654 17 | 18 | using namespace std; 19 | 20 | class PRMGraph { 21 | private: 22 | //node_id (initialized to zero!) 23 | int node_id_; 24 | //record number of degree of freedom 25 | int num_dof_; 26 | //represent vertices as array of joint states 27 | vector> vertices_; 28 | //represent edges as adjacency list 29 | //1st dim = vertex, 2nd dim = neighbors of the vertex 30 | vector> edges_; 31 | 32 | public: 33 | //default constructor 34 | PRMGraph(int numofDOFs); 35 | 36 | int getCurrentNodeID(); 37 | 38 | void addVertex(vector& vertex); 39 | 40 | vector getNodeConfig(int node_id); 41 | 42 | vector getNeighborsID(int node_id); 43 | 44 | void addEdge(int parent_id, int child_id); 45 | 46 | inline double calculateDistance(vector& v1, vector& v2) 47 | { 48 | //calculate L2 (norm) distance between two configurations (vertices) 49 | double total_dist = 0.0; 50 | double joint_dist = 0.0; 51 | for (int i=0; i<(this->num_dof_); i++) { 52 | joint_dist = fabs(v1[i]-v2[i]); 53 | total_dist += (joint_dist < PI) ? pow(joint_dist,2) : pow((2*PI - joint_dist),2); 54 | } 55 | return pow(total_dist, 0.5); 56 | }; 57 | 58 | vector findKNN(vector& new_vertex, int k); 59 | 60 | int getNearestVertex(vector& new_vertex); 61 | }; 62 | 63 | #endif -------------------------------------------------------------------------------- /rrt_tree.cpp: -------------------------------------------------------------------------------- 1 | #include "rrt_tree.h" 2 | 3 | RRTTree::RRTTree(int numofDOFs) 4 | : num_dof_(numofDOFs), 5 | node_id_(0) { } 6 | 7 | 8 | int RRTTree::getNodeID() 9 | { 10 | return this->node_id_; 11 | } 12 | 13 | vector RRTTree::getNodeConfig(int node_id) 14 | { 15 | return (this->vertices_)[node_id]; 16 | } 17 | 18 | // int RRTTree::getParentVertex(int node_id) 19 | // { 20 | // return (this->edges_)[node_id]; 21 | // } 22 | 23 | void RRTTree::removeEdge(int child_node_id) 24 | { 25 | (this->edges_).erase(child_node_id); 26 | return; 27 | } 28 | 29 | void RRTTree::addVertex(vector& vertex) 30 | { 31 | assert(vertex.size() == this->num_dof_); 32 | 33 | // add one row 34 | (this->vertices_).push_back(vector()); 35 | // fill in the new row 36 | vertices_[this->node_id_] = vertex; 37 | 38 | (this->node_id_)++; 39 | } 40 | 41 | double RRTTree::getVertexCost(int node_id) 42 | { 43 | assert(node_id < (this.costs_).size()); 44 | return (this->costs_)[node_id]; 45 | } 46 | 47 | void RRTTree::setVertexCost(int node_id, double cost) 48 | { 49 | if (node_id < (this->costs_).size()) { 50 | // update cost for exisiting vertex 51 | (this->costs_)[node_id] = cost; 52 | } else { 53 | // record cost for new vertex 54 | assert(node_id == this->node_id_-1); 55 | (this->costs_).push_back(cost); 56 | } 57 | } 58 | 59 | void RRTTree::addEdge(int parent_id, int child_id) 60 | { 61 | assert((this->edges_).find(child_id) == (this->edges_).end()); 62 | (this->edges_)[child_id] = parent_id; 63 | } 64 | 65 | int RRTTree::getNearestVertex(vector& new_vertex) 66 | { 67 | double min_dist = DBL_MAX; 68 | int min_index = -1; 69 | double curr_dist = 0.0; 70 | // perform linear search to get the nearest neighbor 71 | for (int i=0; i<(this->vertices_).size(); i++) { 72 | curr_dist = this->calculateDistance(new_vertex, (this->vertices_)[i]); 73 | if (curr_dist < min_dist) { 74 | min_dist = curr_dist; 75 | min_index = i; 76 | } 77 | } 78 | return min_index; 79 | } 80 | 81 | vector RRTTree::getNearVertices(int node_id, double radius) 82 | { 83 | //perform linear search to get neighbors within distance r to current vertex 84 | vector neighbors_id; 85 | double curr_dist = 0.0; 86 | for (int i=0; i<(this->vertices_).size(); i++) { 87 | curr_dist = this->calculateDistance((this->vertices_)[node_id], (this->vertices_)[i]); 88 | if (curr_dist <= radius) neighbors_id.push_back(i); 89 | } 90 | return neighbors_id; 91 | } 92 | 93 | vector RRTTree::returnPlan() 94 | { 95 | // return the vertices' ids along the path 96 | vector plan_vertices; 97 | int index_iter = this->node_id_-1; 98 | while (index_iter) { 99 | plan_vertices.push_back(index_iter); 100 | index_iter = (this->edges_)[index_iter]; 101 | } 102 | // add the start configuration 103 | plan_vertices.push_back(0); 104 | return plan_vertices; 105 | } 106 | 107 | vector RRTTree::returnPlan(int node_id) 108 | { 109 | // return the vertices' ids along the path 110 | vector plan_vertices; 111 | int index_iter = node_id; 112 | while (index_iter) { 113 | plan_vertices.push_back(index_iter); 114 | index_iter = (this->edges_)[index_iter]; 115 | } 116 | // add the start configuration 117 | plan_vertices.push_back(0); 118 | return plan_vertices; 119 | } -------------------------------------------------------------------------------- /rrt_tree.h: -------------------------------------------------------------------------------- 1 | #ifndef RRTTREE_H 2 | #define RRTTREE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | /**************************************************** 11 | Class for creating and maintaining a growing tree 12 | structure used by RRT/RRT-connect/RRT* algorithms 13 | ****************************************************/ 14 | 15 | #define PI 3.141592654 16 | 17 | using namespace std; 18 | 19 | class RRTTree { 20 | private: 21 | //node_id (initialized to zero!) 22 | int node_id_; 23 | //record number of degree of freedom 24 | int num_dof_; 25 | //represent vertices as array of joint states 26 | vector> vertices_; 27 | //represent edge by its vertices' id 28 | //key = child_id, value = parent_id 29 | unordered_map edges_; 30 | //cost associated with each vertex (for RRT* only) 31 | vector costs_; 32 | 33 | public: 34 | //default constructor 35 | RRTTree(int numofDOFs); 36 | 37 | int getNodeID(); 38 | 39 | vector getNodeConfig(int node_id); 40 | 41 | //int getParentVertex(int node_id); 42 | 43 | void removeEdge(int child_node_id); 44 | 45 | void addVertex(vector& vertex); 46 | 47 | double getVertexCost(int node_id); 48 | 49 | void setVertexCost(int node_id, double cost); 50 | 51 | void addEdge(int parent_id, int child_id); 52 | 53 | inline double calculateDistance(vector& v1, vector& v2) 54 | { 55 | //calculate distance between two configurations (vertices) 56 | //as the sum of differences between all joint angles 57 | double total_dist = 0.0; 58 | double joint_dist = 0.0; 59 | for (int i=0; i<(this->num_dof_); i++) { 60 | joint_dist = fabs(v1[i]-v2[i]); 61 | total_dist += (joint_dist < PI) ? joint_dist : (2*PI - joint_dist); 62 | } 63 | return total_dist; 64 | }; 65 | 66 | int getNearestVertex(vector& new_vertex); 67 | 68 | vector getNearVertices(int node_id, double radius); 69 | 70 | vector returnPlan(); 71 | 72 | vector returnPlan(int node_id); 73 | 74 | }; 75 | 76 | #endif -------------------------------------------------------------------------------- /runtest.m: -------------------------------------------------------------------------------- 1 | function[numofmoves, caught] = runtest(mapfile, armstart, armgoal, planner_id) 2 | 3 | LINKLENGTH_CELLS=10; 4 | envmap = load(mapfile); 5 | 6 | close all; 7 | 8 | %draw the environment 9 | image(envmap'*255); 10 | hold on; 11 | 12 | %armplan should be a matrix of D by N 13 | %where D is the number of DOFs in the arm (length of armstart) and 14 | %N is the number of steps in the plan 15 | armplan = armplanner(envmap, armstart, armgoal, planner_id); 16 | 17 | fprintf(1, 'plan of length %d was found\n', size(armplan,1)); 18 | 19 | %draw the plan 20 | midx = size(envmap,2)/2; 21 | x = zeros(length(armstart)+1,1); 22 | x(1) = midx; 23 | y = zeros(length(armstart)+1,1); 24 | for i = 1:size(armplan) 25 | for j = 1:length(armstart) 26 | x(j+1) = x(j) + LINKLENGTH_CELLS*cos(armplan(i,j)); 27 | y(j+1) = y(j) + LINKLENGTH_CELLS*sin(armplan(i,j)); 28 | end; 29 | plot(x,y, 'c-'); 30 | pause(0.1); 31 | end; 32 | 33 | %armplan --------------------------------------------------------------------------------