├── robot.mat ├── img ├── random.gif ├── after_prun.gif ├── dynamic_1.gif ├── dynamic_2.gif ├── before_prun.gif └── epsilon_greedy.gif ├── Extend.m ├── RandomNode.m ├── map ├── map1.txt ├── map3.txt ├── map4.txt ├── map5.txt ├── example_map.txt ├── map7.txt ├── map8.txt ├── map2.txt └── map6.txt ├── sample.m ├── DetCol.m ├── Neighbor.m ├── readme.md ├── Regrow.m ├── runsim.m ├── path_opt.m └── SRRT.m /robot.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shumin326/Manipulator/HEAD/robot.mat -------------------------------------------------------------------------------- /img/random.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shumin326/Manipulator/HEAD/img/random.gif -------------------------------------------------------------------------------- /img/after_prun.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shumin326/Manipulator/HEAD/img/after_prun.gif -------------------------------------------------------------------------------- /img/dynamic_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shumin326/Manipulator/HEAD/img/dynamic_1.gif -------------------------------------------------------------------------------- /img/dynamic_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shumin326/Manipulator/HEAD/img/dynamic_2.gif -------------------------------------------------------------------------------- /img/before_prun.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shumin326/Manipulator/HEAD/img/before_prun.gif -------------------------------------------------------------------------------- /img/epsilon_greedy.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shumin326/Manipulator/HEAD/img/epsilon_greedy.gif -------------------------------------------------------------------------------- /Extend.m: -------------------------------------------------------------------------------- 1 | function q_new = Extend(q_curr,q_next) 2 | % extend the current node some distance to the next node 3 | yita = randi([1,7])/10; 4 | q_new.val = q_curr.val+yita*(q_next.val-q_curr.val); 5 | q_new.parent = q_curr; 6 | q_new.flag = 1; 7 | end -------------------------------------------------------------------------------- /RandomNode.m: -------------------------------------------------------------------------------- 1 | function q = RandomNode(q_start,q_goal) 2 | q.val = zeros(1,6); 3 | q.parent = []; 4 | q.flag = 1; 5 | q.val(1) = randi([-14,14])/10; 6 | q.val(2) = randi([-12,14])/10; 7 | q.val(3) = randi([-18,17])/10; 8 | q.val(4) = randi([-19,17])/10; 9 | end -------------------------------------------------------------------------------- /map/map1.txt: -------------------------------------------------------------------------------- 1 | # Map 1 2 | # 3 | # usage: 4 | # element x_min y_min z_min x_max y_max z_max 5 | # 6 | # you must specify the boundary of your map using the 'boundary' element 7 | # and can optionally include obstacles using the 'block' element 8 | 9 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 10 | 11 | block 100 -300 106.825 400 300 113.175 12 | 13 | -------------------------------------------------------------------------------- /map/map3.txt: -------------------------------------------------------------------------------- 1 | # example map 2 | # 3 | # usage: 4 | # element x_min y_min z_min x_max y_max z_max 5 | # 6 | # you must specify the boundary of your map using the 'boundary' element 7 | # and can optionally include obstacles using the 'block' element 8 | 9 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 10 | 11 | block 100 100 0 300 300 500 12 | #block -300 -300 400 300 300 500 13 | -------------------------------------------------------------------------------- /map/map4.txt: -------------------------------------------------------------------------------- 1 | # example map 2 | # 3 | # usage: 4 | # element x_min y_min z_min x_max y_max z_max 5 | # 6 | # you must specify the boundary of your map using the 'boundary' element 7 | # and can optionally include obstacles using the 'block' element 8 | 9 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 10 | 11 | 12 | block 200 -300 300 400 300 400 13 | block 100 -300 -200 200 100 200 14 | -------------------------------------------------------------------------------- /map/map5.txt: -------------------------------------------------------------------------------- 1 | # example map 2 | # 3 | # usage: 4 | # element x_min y_min z_min x_max y_max z_max 5 | # 6 | # you must specify the boundary of your map using the 'boundary' element 7 | # and can optionally include obstacles using the 'block' element 8 | 9 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 10 | 11 | 12 | block 200 -300 300 400 300 400 13 | block 100 -300 -200 200 100 200 14 | -------------------------------------------------------------------------------- /map/example_map.txt: -------------------------------------------------------------------------------- 1 | # example map 2 | # 3 | # usage: 4 | # element x_min y_min z_min x_max y_max z_max 5 | # 6 | # you must specify the boundary of your map using the 'boundary' element 7 | # and can optionally include obstacles using the 'block' element 8 | 9 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 10 | 11 | block 100 100 0 300 300 500 12 | block -300 -300 400 300 300 500 13 | -------------------------------------------------------------------------------- /map/map7.txt: -------------------------------------------------------------------------------- 1 | # example map 2 | # 3 | # usage: 4 | # element x_min y_min z_min x_max y_max z_max 5 | # 6 | # you must specify the boundary of your map using the 'boundary' element 7 | # and can optionally include obstacles using the 'block' element 8 | 9 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 10 | 11 | 12 | block 100 -300 1 200 100 200 13 | block 200 -300 300 400 300 400 14 | 15 | -------------------------------------------------------------------------------- /sample.m: -------------------------------------------------------------------------------- 1 | function q = sample(q_start,q_goal,sr) 2 | p = rand(); 3 | % those are different epsilon functions 4 | % epsilon = (exp(2*sr)-1)/(1.25*exp(2));%SR1 5 | % epsilon = (exp(2*sr)-1)/(1.58*exp(2));%SR2 6 | % epsilon = (exp(2*sr)-1)/exp(2);%SR3 7 | epsilon = (exp(2*sr)-1)/(0.87*exp(2));%SR4 8 | if p>epsilon 9 | q = RandomNode(q_start,q_goal); 10 | else 11 | q = q_goal; 12 | end 13 | end 14 | 15 | 16 | -------------------------------------------------------------------------------- /map/map8.txt: -------------------------------------------------------------------------------- 1 | # example map 2 | # 3 | # usage: 4 | # element x_min y_min z_min x_max y_max z_max 5 | # 6 | # you must specify the boundary of your map using the 'boundary' element 7 | # and can optionally include obstacles using the 'block' element 8 | 9 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 10 | 11 | 12 | block 150 60 0 400 66.7 350.0 13 | block -400 -200 400 400 200. 400.7 14 | block 150 160 0 450 166.7 400.0 15 | -------------------------------------------------------------------------------- /map/map2.txt: -------------------------------------------------------------------------------- 1 | # Map 2 2 | # 3 | # This map is a narrow passageway 4 | # 5 | # usage: 6 | # element x_min y_min z_min x_max y_max z_max 7 | # 8 | # you must specify the boundary of your map using the 'boundary' element 9 | # and can optionally include obstacles using the 'block' element 10 | 11 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 12 | 13 | block 150 60 -50.0 400 66.7 350.0 14 | block 150 -66.7 -50.0 400 -60 350.0 15 | 16 | #block -300 -300 400 300 300 500 17 | -------------------------------------------------------------------------------- /map/map6.txt: -------------------------------------------------------------------------------- 1 | # example map 2 | # 3 | # usage: 4 | # element x_min y_min z_min x_max y_max z_max 5 | # 6 | # you must specify the boundary of your map using the 'boundary' element 7 | # and can optionally include obstacles using the 'block' element 8 | 9 | boundary -400.0 -400.0 -200.0 400.0 400.0 500.0 10 | 11 | 12 | block 200 -200 300 300 -100 400 13 | block 100 -100 -50 200 0 50 14 | block 100 -100 -200 200 -50 -100 15 | block 300 0.0 0.0 400 100 150 16 | block 50 -250 150 100 -150 200 17 | -------------------------------------------------------------------------------- /DetCol.m: -------------------------------------------------------------------------------- 1 | function isCol = DetCol(q_near,q_new,map) 2 | load 'robot.mat' robot 3 | % consider the case when input is not struct array 4 | Dq1 = isstruct(q_near); 5 | Dq2 = isstruct(q_new); 6 | if Dq1 7 | q_near = q_near.val; 8 | end 9 | if Dq2 10 | q_new = q_new.val; 11 | end 12 | % choose 20 inter-points and check them one by one 13 | n = 50; 14 | isCol = 0; 15 | t = linspace(0,1,n)'; 16 | morePath = (1-t)*q_near + t*q_new; 17 | for jj = 1:size(morePath,1) 18 | if isRobotCollided(morePath(jj,:),map,robot) 19 | isCol=1; 20 | return 21 | end 22 | end 23 | end -------------------------------------------------------------------------------- /Neighbor.m: -------------------------------------------------------------------------------- 1 | function q_near = Neighbor(q_curr,T) 2 | %% find the closest valid node in T w.r.t current q 3 | % input. current q: q_curr, Tree T 4 | % output. closest q(struct) 5 | [n,m] = size(T); 6 | q_near = T(1); 7 | min_dis = norm(q_near.val-q_curr.val); 8 | if n>2 9 | for i=2:n 10 | %choose neighbors nodes that are valid 11 | if T(i).flag==1 12 | q = T(i); 13 | dis = norm(q.val-q_curr.val); 14 | if dis1 35 | % len = zeros((n-1),1); 36 | % w = zeros(n,3); 37 | % for i=1:n 38 | % ans = calculateFK_sol(path(i,:)); 39 | % w(i,:)= ans(6,:); 40 | % end 41 | % for i=1:n-1 42 | % len(i)=norm(w(i,:)-w(i+1,:)); 43 | % end 44 | % total_len = total_len + sum(len); 45 | % total_step = total_step + sum(len)/(n-1); 46 | % % maxDiff = maxDiff+ (max(len)-min(len)); 47 | % end 48 | % if mod(j,10)==0 49 | % j 50 | % end 51 | % s_rate = s_rate + success; 52 | % end 53 | % maxDiff 54 | % total_len 55 | % total_step 56 | % toc; 57 | % s_rate = s_rate/100 58 | 59 | 60 | 61 | %% Run the simulation 62 | profile off 63 | 64 | %% Plot the output 65 | 66 | % plotLynxPath(map,path_plot,10); 67 | 68 | profile viewer 69 | -------------------------------------------------------------------------------- /path_opt.m: -------------------------------------------------------------------------------- 1 | function [path_new,T_new] = path_opt(path,map,T) 2 | %% input: oringinal path obtained from RRT(n*6, struct) 3 | % output: optimized path(n*6,struct) 4 | path_new = prun(path,map); 5 | [path_new,T_new] = expand(path_new,map,T); 6 | end 7 | %% prun the path 8 | function path_new = prun(path,map) 9 | %% input: oringinal path obtained from RRT(n*6, struct) 10 | % output: pruned path(n*6,struct) 11 | path_new = path(1); 12 | [n,m] = size(path); 13 | i = 1; 14 | %if we only have two sample points: 15 | if n<3 16 | path_new = path; 17 | else 18 | %we have 3 or more sample points: 19 | while i max_length 50 | % if the length of connection link is bigger than max_length, add a mid point 51 | insert.val = (prunned_path(i).val+prunned_path(i+1).val)/2; 52 | for j=1:50 53 | % if the mid point contains collision, try some random nodes 54 | insert.val = prunned_path(i).val+(prunned_path(i+1).val-prunned_path(i).val)*randi([30,70])/100; 55 | DetCol(insert,prunned_path(i),map) 56 | if ~DetCol(insert,prunned_path(i),map) 57 | break 58 | end 59 | end 60 | if j<50 61 | % add inter-points into the path and tree, set the correct parents to 62 | % the new node and the following node 63 | insert.parent = prunned_path(i); 64 | insert.flag = 1; 65 | prunned_path(i+1).parent = insert; 66 | path_new = [path_new;insert]; 67 | T_new = [T_new;insert]; 68 | end 69 | end 70 | path_new = [path_new;prunned_path(i+1)]; 71 | end 72 | end 73 | 74 | -------------------------------------------------------------------------------- /SRRT.m: -------------------------------------------------------------------------------- 1 | function [maximum_try,SR,T,path,path_plot,success] = SRRT(start,goal,map) 2 | %% generative function for SRRT planner 3 | % input: start point:start, shape:1,6 4 | % end point:goal, shape:1,6 5 | % map 6 | load 'robot.mat' robot 7 | %initialzation 8 | success = 1; 9 | path = []; 10 | path_plot = []; 11 | epsilon = 0.07; 12 | q_start.val = start; 13 | q_goal.val = goal; 14 | q_start.parent = []; 15 | q_goal.parent = []; 16 | q_start.flag = 1; 17 | q_goal.flag = 1; 18 | T = q_start; 19 | %counter for success sampling rate 20 | s = 0; 21 | %sample success sample rate, it determines the adaptive threshold epsilon 22 | sr = 0.5; 23 | % recorder for success sample rate 24 | SR = []; 25 | maximum_try = 1; 26 | %% main 27 | % check if end configuration is feasible 28 | if isRobotCollided(q_goal.val,map,robot) 29 | success = 0; 30 | disp('invalid goal position') 31 | return 32 | end 33 | %set maximum sample tries to be 500, if exceeds, return success=0 34 | while maximum_try<501 35 | % sample nodes in the space 36 | q_next = sample(q_start,q_goal,sr); 37 | % find the nearest node 38 | q_near = Neighbor(q_next,T); 39 | % entend the node towards q_next 40 | q_new = Extend(q_near,q_next); 41 | % check if new node is collision free 42 | isCol = DetCol(q_near,q_new,map); 43 | % if new node is collision free, add it to the tree, set the nearest 44 | % node as its parent and update the success sample rate: sr 45 | if isCol==0 46 | T = [T;q_new]; 47 | s = s+1; 48 | sr = s/maximum_try; 49 | SR = [SR,sr]; 50 | else 51 | sr = s/maximum_try; 52 | SR = [SR,sr]; 53 | end 54 | % stop condition 1: new node is within epsilon distance to end 55 | % configuration 56 | if norm(q_new.val-q_goal.val)< epsilon 57 | break; 58 | end 59 | %stop condition 2: connection of new node and end configuration 60 | %contains no collision 61 | if ~DetCol(q_new,q_goal,map) 62 | break 63 | end 64 | maximum_try = maximum_try + 1; 65 | end 66 | % if try 500 times, return failure 67 | if maximum_try == 501 68 | success = 0; 69 | return 70 | end 71 | % find the path by iteration 72 | q = q_new; 73 | while ~isequal(q.val,q_start.val) 74 | path = [q;path]; 75 | q = q.parent; 76 | end 77 | %add start and end goal to path since the last node might not be within 78 | %epsilon distance to end configuration 79 | path = [q_start;path;q_goal]; 80 | path = path_opt(path,map,T); 81 | [a,b] = size(path); 82 | % get the values of path and return it as path_plot. it's for plotting 83 | for i= 1:a 84 | path_plot = [path_plot;path(i).val]; 85 | end 86 | end 87 | --------------------------------------------------------------------------------