├── print.pdf ├── grid_14x14.png ├── grid2coord.m ├── README.md ├── permn.m ├── rbk_inflation_3d.m └── rbk_inflation.m /print.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WenchaoDing/kinodynamic_replanning/HEAD/print.pdf -------------------------------------------------------------------------------- /grid_14x14.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WenchaoDing/kinodynamic_replanning/HEAD/grid_14x14.png -------------------------------------------------------------------------------- /grid2coord.m: -------------------------------------------------------------------------------- 1 | function coord_path = grid2coord(path, grid_center_coord) 2 | [path_size, num_dim] = size(path); 3 | coord_path = zeros(path_size,num_dim); 4 | for i = 1:1:path_size 5 | if num_dim == 2 6 | coord_path(i,:) = grid_center_coord{path(i,1), path(i,2)}; 7 | else 8 | coord_path(i,:) = grid_center_coord{path(i,1), path(i,2), path(i,3)}; 9 | end 10 | end 11 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kinodynamic_replanning 2 | Materials for the paper "Trajectory Replanning for Quadrotors Using Kinodynamic Search and Elastic Optimization", which is submitted to ICRA2018. 3 | ## File Discription 4 | * contain_linear_segments.nb 5 | 6 | The file is to solve the max-min problem in Appendix C in closed form, which is written in [Mathematica](https://www.wolfram.com/mathematica/) and has been tested in Mathematica 10.0. 7 | 8 | * rbk_inflation.m 9 | 10 | The file is to numerically check the inflation needed to guarantee the RBK trajectory to be collision-free, which is written in Matlab and has been tested in Matlab R2016b. 11 | 12 | * rbk_inflation_3d.m 13 | 14 | The file is to show the 3d case, which provides the complementary results. 15 | 16 | * permn.m 17 | 18 | The file is to enumerate all the patterns, which is from [Jos@Mathworks](https://www.mathworks.com/matlabcentral/fileexchange/7147-permn-v--n--k-). 19 | 20 | * grid2coord.m 21 | 22 | Some basic coordinate transformation. 23 | 24 | * print.pdf 25 | 26 | For the users who do not have [Mathematica](https://www.wolfram.com/mathematica/) installed, we print the script and results from [Mathematica](https://www.wolfram.com/mathematica/) for them. 27 | 28 | 29 | ## Authors 30 | * Wenchao DING 31 | * William Wu 32 | 33 | HKUST Robotics Institute 34 | -------------------------------------------------------------------------------- /permn.m: -------------------------------------------------------------------------------- 1 | function [M, I] = permn(V, N, K) 2 | % PERMN - permutations with repetition 3 | % Using two input variables V and N, M = PERMN(V,N) returns all 4 | % permutations of N elements taken from the vector V, with repetitions. 5 | % V can be any type of array (numbers, cells etc.) and M will be of the 6 | % same type as V. If V is empty or N is 0, M will be empty. M has the 7 | % size numel(V).^N-by-N. 8 | % 9 | % When only a subset of these permutations is needed, you can call PERMN 10 | % with 3 input variables: M = PERMN(V,N,K) returns only the K-ths 11 | % permutations. The output is the same as M = PERMN(V,N) ; M = M(K,:), 12 | % but it avoids memory issues that may occur when there are too many 13 | % combinations. This is particulary useful when you only need a few 14 | % permutations at a given time. If V or K is empty, or N is zero, M will 15 | % be empty. M has the size numel(K)-by-N. 16 | % 17 | % [M, I] = PERMN(...) also returns an index matrix I so that M = V(I). 18 | % 19 | % Examples: 20 | % M = permn([1 2 3],2) % returns the 9-by-2 matrix: 21 | % 1 1 22 | % 1 2 23 | % 1 3 24 | % 2 1 25 | % 2 2 26 | % 2 3 27 | % 3 1 28 | % 3 2 29 | % 3 3 30 | % 31 | % M = permn([99 7],4) % returns the 16-by-4 matrix: 32 | % 99 99 99 99 33 | % 99 99 99 7 34 | % 99 99 7 99 35 | % 99 99 7 7 36 | % ... 37 | % 7 7 7 99 38 | % 7 7 7 7 39 | % 40 | % M = permn({'hello!' 1:3},2) % returns the 4-by-2 cell array 41 | % 'hello!' 'hello!' 42 | % 'hello!' [1x3 double] 43 | % [1x3 double] 'hello!' 44 | % [1x3 double] [1x3 double] 45 | % 46 | % V = 11:15, N = 3, K = [2 124 21 99] 47 | % M = permn(V, N, K) % returns the 4-by-3 matrix: 48 | % % 11 11 12 49 | % % 15 15 14 50 | % % 11 15 11 51 | % % 14 15 14 52 | % % which are the 2nd, 124th, 21st and 99th permutations 53 | % % Check with PERMN using two inputs 54 | % M2 = permn(V,N) ; isequal(M2(K,:),M) 55 | % % Note that M2 is a 125-by-3 matrix 56 | % 57 | % % PERMN can be used generate a binary table, as in 58 | % B = permn([0 1],5) 59 | % 60 | % NB Matrix sizes increases exponentially at rate (n^N)*N. 61 | % 62 | % See also PERMS, NCHOOSEK 63 | % ALLCOMB, PERMPOS on the File Exchange 64 | 65 | % tested in Matlab 2016a 66 | % version 6.1 (may 2016) 67 | % (c) Jos van der Geest 68 | % Matlab File Exchange Author ID: 10584 69 | % email: samelinoa@gmail.com 70 | 71 | % History 72 | % 1.1 updated help text 73 | % 2.0 new faster algorithm 74 | % 3.0 (aug 2006) implemented very fast algorithm 75 | % 3.1 (may 2007) Improved algorithm Roger Stafford pointed out that for some values, the floor 76 | % operation on floating points, according to the IEEE 754 standard, could return 77 | % erroneous values. His excellent solution was to add (1/2) to the values 78 | % of A. 79 | % 3.2 (may 2007) changed help and error messages slightly 80 | % 4.0 (may 2008) again a faster implementation, based on ALLCOMB, suggested on the 81 | % newsgroup comp.soft-sys.matlab on May 7th 2008 by "Helper". It was 82 | % pointed out that COMBN(V,N) equals ALLCOMB(V,V,V...) (V repeated N 83 | % times), ALLCMOB being faster. Actually version 4 is an improvement 84 | % over version 1 ... 85 | % 4.1 (jan 2010) removed call to FLIPLR, using refered indexing N:-1:1 86 | % (is faster, suggestion of Jan Simon, jan 2010), removed REPMAT, and 87 | % let NDGRID handle this 88 | % 4.2 (apr 2011) corrrectly return a column vector for N = 1 (error pointed 89 | % out by Wilson). 90 | % 4.3 (apr 2013) make a reference to COMBNSUB 91 | % 5.0 (may 2015) NAME CHANGED (COMBN -> PERMN) and updated description, 92 | % following comment by Stephen Obeldick that this function is misnamed 93 | % as it produces permutations with repetitions rather then combinations. 94 | % 5.1 (may 2015) always calculate M via indices 95 | % 6.0 (may 2015) merged the functionaly of permnsub (aka combnsub) and this 96 | % function 97 | % 6.1 (may 2016) fixed spelling errors 98 | 99 | narginchk(2,3) ; 100 | 101 | if fix(N) ~= N || N < 0 || numel(N) ~= 1 ; 102 | error('permn:negativeN','Second argument should be a positive integer') ; 103 | end 104 | nV = numel(V) ; 105 | 106 | if nargin==2, % PERMN(V,N) - return all permutations 107 | 108 | if nV==0 || N == 0, 109 | M = zeros(nV,N) ; 110 | I = zeros(nV,N) ; 111 | 112 | elseif N == 1, 113 | % return column vectors 114 | M = V(:) ; 115 | I = (1:nV).' ; 116 | else 117 | % this is faster than the math trick used for the call with three 118 | % arguments. 119 | [Y{N:-1:1}] = ndgrid(1:nV) ; 120 | I = reshape(cat(N+1,Y{:}),[],N) ; 121 | M = V(I) ; 122 | end 123 | else % PERMN(V,N,K) - return a subset of all permutations 124 | nK = numel(K) ; 125 | if nV == 0 || N == 0 || nK == 0 126 | M = zeros(numel(K), N) ; 127 | I = zeros(numel(K), N) ; 128 | elseif nK < 1 || any(K<1) || any(K ~= fix(K)) 129 | error('permn:InvalidIndex','Third argument should contain positive integers.') ; 130 | else 131 | 132 | V = reshape(V,1,[]) ; % v1.1 make input a row vector 133 | nV = numel(V) ; 134 | Npos = nV^N ; 135 | if any(K > Npos) 136 | warning('permn:IndexOverflow', ... 137 | 'Values of K exceeding the total number of combinations are saturated.') 138 | K = min(K, Npos) ; 139 | end 140 | 141 | % The engine is based on version 3.2 with the correction 142 | % suggested by Roger Stafford. This approach uses a single matrix 143 | % multiplication. 144 | B = nV.^(1-N:0) ; 145 | I = ((K(:)-.5) * B) ; % matrix multiplication 146 | I = rem(floor(I),nV) + 1 ; 147 | M = V(I) ; 148 | end 149 | end 150 | 151 | % Algorithm using for-loops 152 | % which can be implemented in C or VB 153 | 154 | % nv = length(V) ; 155 | % C = zeros(nv^N,N) ; % declaration 156 | % for ii=1:N, 157 | % cc = 1 ; 158 | % for jj=1:(nv^(ii-1)), 159 | % for kk=1:nv, 160 | % for mm=1:(nv^(N-ii)), 161 | % C(cc,ii) = V(kk) ; 162 | % cc = cc + 1 ; 163 | % end 164 | % end 165 | % end 166 | % end 167 | 168 | -------------------------------------------------------------------------------- /rbk_inflation_3d.m: -------------------------------------------------------------------------------- 1 | clc; clear all; close all; 2 | %% parameters 3 | cell_size_row = 0.16; 4 | cell_size_col = 0.16; 5 | cell_size_height = 0.16; 6 | grid_size_row = 14; 7 | grid_size_col = 14; 8 | grid_size_height = 14; 9 | %visualize occupancy grid 10 | POLYORDER = 5; 11 | 12 | grid_center_coord = cell(grid_size_row, grid_size_col, grid_size_height); 13 | temp_coord = zeros(3,1); 14 | for i = 1:1:grid_size_row 15 | for j = 1:1:grid_size_col 16 | for k = 1:1:grid_size_height 17 | %compute pixel index 18 | temp_coord(1) = (j - 1) * cell_size_col + 0.5 * cell_size_col; 19 | temp_coord(2) = (i - 1) * cell_size_row + 0.5 * cell_size_row; 20 | temp_coord(3) = (k - 1) * cell_size_height + 0.5 * cell_size_height; 21 | grid_center_coord{i,j,k} = temp_coord; 22 | end 23 | end 24 | end 25 | 26 | kBasisMat =[ 1 26 66 26 1 0; 27 | -5 -50 0 50 5 0; 28 | 10 20 -60 20 10 0; 29 | -10 20 0 -20 10 0; 30 | 5 -20 30 -20 5 0; 31 | -1 5 -10 10 -5 1;]; 32 | kBasisMat = kBasisMat/factorial(5); 33 | 34 | %% check all the patterns 35 | 36 | start_index = [7 7 7]; %locate initial point at center so that all the patterns are on the grid 37 | direction_map = [ 38 | 1 1 -1; 39 | 1 1 0; 40 | 1 1 1; 41 | 1 0 -1; 42 | 1 0 0; 43 | 1 0 1; 44 | 1 -1 -1; 45 | 1 -1 0; 46 | 1 -1 1; 47 | 0 0 -1; 48 | 0 0 1; 49 | 0 -1 -1; 50 | 0 -1 0; 51 | 0 -1 1; 52 | 0 1 -1; 53 | 0 1 0; 54 | 0 1 1; 55 | -1 -1 -1; 56 | -1 -1 0; 57 | -1 -1 1; 58 | -1 0 -1; 59 | -1 0 0; 60 | -1 0 1; 61 | -1 1 -1; 62 | -1 1 0; 63 | -1 1 1; 64 | ]; 65 | 66 | num_connect = size(direction_map,1); 67 | num_gen = POLYORDER; 68 | 69 | combs = permn(1:num_connect, num_gen); 70 | num_total_comb = size(combs,1); 71 | pattern_coord = cell(num_total_comb,1); 72 | pattern_index = cell(num_total_comb,1); 73 | 74 | %for B-spline evaluation 75 | ut_step = 0.02; 76 | utVec = 0: ut_step :1.0; 77 | %delta_vec = 0.0: 0.01: 0.5 * cell_size_row; 78 | delta_vec = 0.2 * cell_size_row; 79 | 80 | run = 0; %turn on this switch to run this part, may need a long time to output results. 81 | %or you can find the results generated below. 82 | if run 83 | disp('Start to generate pattern info..'); 84 | for i = 1 : num_total_comb 85 | %for each comb/pattern 86 | current_pattern = zeros(num_gen + 1, 3); 87 | current_pattern(1,:) = start_index; 88 | current_comb = combs(i,:); 89 | for step = 1 : num_gen 90 | current_pattern(step+1,:) = current_pattern(step,: ) + direction_map(current_comb(step),:); 91 | end 92 | %transform current_pattern to coord 93 | current_coord_pattern = grid2coord(current_pattern, grid_center_coord); 94 | %store all the patterns 95 | pattern_coord{i} = current_coord_pattern; 96 | pattern_index{i} = current_pattern; 97 | end 98 | 99 | disp('Pattern info recorded..'); 100 | 101 | 102 | for delta = delta_vec 103 | 104 | max_pattern_deviation = -inf; %compute the maximum deviation for all possible patterns 105 | max_pattern_index = 0; 106 | 107 | is_exceed = 0; %for each delta, by default exceed flag is false; 108 | for i = 1: num_total_comb 109 | disp(['current comb: ' num2str(i) ]); 110 | disp(['Total comb: ' num2str(num_total_comb) ]); 111 | pattern_deviation = -inf; %for each pattern, compute the deviation 112 | pattern = pattern_coord{i}; 113 | current_index = pattern_index{i}; 114 | for ut = utVec % for each sampling point of the trajectory of the pattern 115 | sample_deviation = inf; %for each sampling point, compute the deviation 116 | is_incell_found = 0; %by default, no in cell is found 117 | p_ut = [1 ut ut^2 ut^3 ut^4 ut^5] * kBasisMat * pattern; %evaluate position 118 | 119 | for k = 1:6 %for all known collision free cells. 120 | temp_coord = pattern(k,:); %get coordinate 121 | if( abs(p_ut(1) -temp_coord(1) ) <= (0.5 * cell_size_row + delta) && ... 122 | abs(p_ut(2) -temp_coord(2) ) <= (0.5 * cell_size_col + delta) && ... 123 | abs(p_ut(3) -temp_coord(3) ) <= (0.5 * cell_size_height + delta)) %find a cell that contains current point with margin delta 124 | is_incell_found = 1; 125 | end 126 | cur_cell_deviaton = max( max(abs(p_ut(1) -temp_coord(1)), abs(p_ut(2) -temp_coord(2))), abs(p_ut(3) -temp_coord(3)) ); %deviation metric 127 | if(cur_cell_deviaton < sample_deviation) 128 | sample_deviation = cur_cell_deviaton; 129 | end 130 | 131 | end 132 | 133 | if(sample_deviation > pattern_deviation) 134 | pattern_deviation = sample_deviation; 135 | end 136 | 137 | if(is_incell_found == 0) %if no cell find, then the sampling point deviates from collision-free space 138 | % disp('deviates from grid..'); 139 | is_exceed = 1; 140 | end 141 | end 142 | if(pattern_deviation > max_pattern_deviation) 143 | max_pattern_deviation = pattern_deviation; 144 | max_pattern_index = i; 145 | end 146 | 147 | end 148 | 149 | delta 150 | max_pattern_deviation 151 | max_pattern_index 152 | max_pattern_show = pattern_index{max_pattern_index} 153 | 154 | if(is_exceed) 155 | disp('Collision-free condition is violated'); 156 | else 157 | disp('All patterns are collision free!'); 158 | end 159 | end 160 | 161 | end 162 | 163 | 164 | % delta = 165 | % 166 | % 0.0320 167 | 168 | % max_pattern_deviation = 169 | % 170 | % 0.0999 171 | 172 | % max_pattern_index = 173 | % 174 | % 2847367 175 | 176 | % max_pattern_show = 177 | % 178 | % 7 7 7 179 | % 8 6 6 180 | % 9 5 5 181 | % 10 6 4 182 | % 11 7 5 183 | % 12 8 6 184 | 185 | %% visualization 186 | 187 | max_pattern_deviation = 0.0999; 188 | 189 | max_pattern_show = [ 190 | 7 7 7 191 | 8 6 6 192 | 9 5 5 193 | 10 6 4 194 | 11 7 5 195 | 12 8 6]; 196 | 197 | disp(['Current inflation level:' ... 198 | num2str(delta_vec(1)) ]); 199 | 200 | disp(['Minimum inflation required:' ... 201 | num2str(max_pattern_deviation - 0.5 * min(min(cell_size_col, cell_size_row), cell_size_height)) ]); 202 | 203 | max_pattern_coord = grid2coord(max_pattern_show, grid_center_coord); 204 | figure; 205 | 206 | 207 | plot3(max_pattern_coord(:,1), max_pattern_coord(:,2),max_pattern_coord(:,3),'.','MarkerSize',10 ); hold on; 208 | plot3(max_pattern_coord(:,1), max_pattern_coord(:,2),max_pattern_coord(:,3),'-gs','LineWidth',2); 209 | 210 | all_points = []; 211 | for ut = utVec % for each sampling point of the trajectory of the pattern 212 | p_ut = [1 ut ut^2 ut^3 ut^4 ut^5] * kBasisMat * max_pattern_coord; %evaluate position 213 | all_points = [all_points; p_ut]; 214 | end 215 | plot3(all_points(:,1), all_points(:,2),all_points(:,3),'r','LineWidth',1); 216 | 217 | set(gca,'Xtick',0:cell_size_col:cell_size_col*grid_size_col); 218 | set(gca,'Ytick',0:cell_size_row:cell_size_row*grid_size_row); 219 | set(gca,'Ztick',0:cell_size_height:cell_size_col*grid_size_height); 220 | set(gca,'XTickLabel',''); 221 | set(gca,'YTickLabel',''); 222 | set(gca,'ZTickLabel',''); 223 | xlim([0 grid_size_col*cell_size_col]); 224 | ylim([0 grid_size_row*cell_size_row]); 225 | zlim([0 grid_size_height*cell_size_height]); 226 | grid on; box on; -------------------------------------------------------------------------------- /rbk_inflation.m: -------------------------------------------------------------------------------- 1 | clc; clear all; close all; 2 | % This program is to check the inflation needed to guarantee that the RBK 3 | % trajectory is collision-free. 4 | % Author: Wenchao DING, HKUST, wdingae@connect.ust.hk 5 | %% paremeters 6 | cell_size_row = 0.16; 7 | cell_size_col = 0.16; 8 | grid_size_row = 14; 9 | grid_size_col = 14; 10 | POLYORDER = 5; 11 | 12 | % input the inflation you what to check 13 | %delta_vec = 0.0: 0.01: 0.03; %check a set of inflation. 14 | delta_vec = 0.2 * cell_size_row; % check a given inflation. 15 | 16 | raw_image = imread('grid_14x14.png'); %load background image 17 | resize_coeff = 0.5; 18 | resized_image = imresize(raw_image,resize_coeff); 19 | gray_image = rgb2gray(resized_image); 20 | [pixel_size_row, pixel_size_col] = size(gray_image); 21 | %for visualization, we use image background 22 | pixel2grid_scale_col = pixel_size_col/grid_size_col; 23 | pixel2grid_scale_row = pixel_size_row/grid_size_row; 24 | 25 | %record the coord of centers. 26 | pixel_center_coord = cell(grid_size_row, grid_size_col); 27 | grid_center_coord = cell(grid_size_row, grid_size_col); 28 | temp_coord = zeros(2,1); 29 | for i = 1:1:grid_size_row 30 | for j = 1:1:grid_size_col 31 | %compute pixel index 32 | temp_coord(1) = (j - 1) * cell_size_col + 0.5 * cell_size_col; 33 | temp_coord(2) = (i - 1) * cell_size_row + 0.5 * cell_size_row; 34 | grid_center_coord{i,j} = temp_coord; % 2d world coordinate 35 | 36 | temp_coord(1) = (j - 1) * pixel2grid_scale_col + 0.5 * pixel2grid_scale_col; 37 | temp_coord(2) = (i - 1) * pixel2grid_scale_row + 0.5 * pixel2grid_scale_row; 38 | pixel_center_coord{i,j} = temp_coord; %image coordinate for visualization and plot 39 | end 40 | end 41 | 42 | kBasisMat =[ 1 26 66 26 1 0; 43 | -5 -50 0 50 5 0; 44 | 10 20 -60 20 10 0; 45 | -10 20 0 -20 10 0; 46 | 5 -20 30 -20 5 0; 47 | -1 5 -10 10 -5 1;]; 48 | kBasisMat = kBasisMat/factorial(5); 49 | 50 | %% check all the patterns 51 | start_index = [7 7]; %locate initial point at center so that all the patterns are on the grid 52 | direction_map = [ 53 | % 0 0; 54 | 1 0; 55 | 1 -1; 56 | 0 -1; 57 | -1 -1; 58 | -1 0; 59 | -1 1; 60 | 0 1; 61 | 1 1; 62 | ]; 63 | 64 | num_connect = size(direction_map,1); 65 | num_gen = POLYORDER; 66 | 67 | combs = permn(1:num_connect, num_gen); 68 | num_total_comb = size(combs,1); 69 | pattern_coord = cell(num_total_comb,1); 70 | pattern_index = cell(num_total_comb,1); 71 | for i = 1 : num_total_comb 72 | %for each comb/pattern 73 | current_pattern = zeros(num_gen + 1, 2); 74 | current_pattern(1,:) = start_index; 75 | current_comb = combs(i,:); 76 | for step = 1 : num_gen 77 | current_pattern(step+1,:) = current_pattern(step,: ) + direction_map(current_comb(step),:); 78 | end 79 | %transform current_pattern to coord 80 | current_coord_pattern = grid2coord(current_pattern, grid_center_coord); 81 | %store all the patterns 82 | pattern_coord{i} = current_coord_pattern; 83 | pattern_index{i} = current_pattern; 84 | end 85 | 86 | %for B-spline evaluation 87 | ut_step = 0.02; 88 | utVec = 0: ut_step :1.0; 89 | idx = 1; 90 | for delta = delta_vec 91 | idx 92 | h = figure(idx); 93 | set(0,'CurrentFigure',h) 94 | rbk_layer = imshow(resized_image); hold on; 95 | idx = idx + 1; 96 | for i = 1:1:grid_size_row 97 | for j = 1:1:grid_size_col 98 | %compute pixel index 99 | temp_coord = pixel_center_coord{i,j}; 100 | 101 | left_top = [temp_coord(1) - (pixel2grid_scale_col - delta * pixel2grid_scale_col/cell_size_col)*0.5,... 102 | temp_coord(2) - (pixel2grid_scale_row - delta * pixel2grid_scale_col/cell_size_col)*0.5 ]; 103 | right_top = [temp_coord(1) + (pixel2grid_scale_col - delta * pixel2grid_scale_col/cell_size_col)*0.5,... 104 | temp_coord(2) - (pixel2grid_scale_row - delta * pixel2grid_scale_col/cell_size_col)*0.5 ]; 105 | right_down = [temp_coord(1) + (pixel2grid_scale_col - delta * pixel2grid_scale_col/cell_size_col)*0.5,... 106 | temp_coord(2) + (pixel2grid_scale_row - delta * pixel2grid_scale_col/cell_size_col)*0.5 ]; 107 | left_down = [temp_coord(1) - (pixel2grid_scale_col - delta * pixel2grid_scale_col/cell_size_col)*0.5,... 108 | temp_coord(2) + (pixel2grid_scale_row - delta * pixel2grid_scale_col/cell_size_col)*0.5 ]; 109 | X = [left_top(1), right_top(1), right_down(1), left_down(1)]; 110 | Y = [left_top(2), right_top(2), right_down(2), left_down(2)]; 111 | fill(X,Y,'b','LineStyle','none'); 112 | end 113 | end 114 | 115 | max_pattern_deviation = -inf; %compute the maximum deviation for all possible patterns 116 | max_pattern_index = 0; 117 | 118 | is_exceed = 0; %for each delta, by default exceed flag is false; 119 | for i = 1: num_total_comb 120 | pattern_deviation = -inf; %for each pattern, compute the deviation 121 | 122 | pattern = pattern_coord{i}; 123 | current_index = pattern_index{i}; 124 | for ut = utVec % for each sampling point of the trajectory of the pattern 125 | sample_deviation = inf; %for each sampling point, compute the deviation 126 | is_incell_found = 0; %by default, no in cell is found 127 | p_ut = [1 ut ut^2 ut^3 ut^4 ut^5] * kBasisMat * pattern; %evaluate position 128 | 129 | for k = 1:6 %for all known collision free cells. 130 | temp_coord = pattern(k,:); %get coordinate 131 | if( abs(p_ut(1) -temp_coord(1) ) <= (0.5 * cell_size_row + delta) && abs(p_ut(2) -temp_coord(2) ) <= (0.5 * cell_size_col+delta) ) %find a cell that contains current point with margin delta 132 | is_incell_found = 1; 133 | end 134 | cur_cell_deviaton = max( abs(p_ut(1) -temp_coord(1) ), abs(p_ut(2) -temp_coord(2) ) ); %deviation metric (l-infinite norn) 135 | if(cur_cell_deviaton < sample_deviation) 136 | sample_deviation = cur_cell_deviaton; 137 | end 138 | 139 | end 140 | 141 | if(sample_deviation > pattern_deviation) 142 | pattern_deviation = sample_deviation; 143 | end 144 | 145 | if(is_incell_found == 0) %if no cell find, then the sampling point deviates from collision-free space 146 | % disp('deviates from grid..'); 147 | is_exceed = 1; 148 | end 149 | end 150 | if(pattern_deviation > max_pattern_deviation) 151 | max_pattern_deviation = pattern_deviation; 152 | max_pattern_index = i; 153 | end 154 | 155 | end 156 | disp(['Current inflation level:' ... 157 | num2str(delta) ]); 158 | 159 | disp(['Minimum inflation required:' ... 160 | num2str(max_pattern_deviation - 0.5 * min(cell_size_col, cell_size_row) ) ]); 161 | 162 | disp('The pattern that has the maximum deviation:'); 163 | max_pattern_show = pattern_index{max_pattern_index} 164 | 165 | max_pattern_grid_coord = zeros(size(max_pattern_show) ); 166 | max_pattern_pixel_coord = zeros(size(max_pattern_show) ); 167 | 168 | for i = 1 : size(max_pattern_grid_coord,1) 169 | max_pattern_grid_coord(i,:) = grid_center_coord{max_pattern_show(i,1), max_pattern_show(i,2)}; 170 | max_pattern_pixel_coord(i,:) = pixel_center_coord{max_pattern_show(i,1), max_pattern_show(i,2)}; 171 | 172 | left_top = [max_pattern_pixel_coord(i,1) - (pixel2grid_scale_col + delta * pixel2grid_scale_col/cell_size_col)*0.5,... 173 | max_pattern_pixel_coord(i,2) - (pixel2grid_scale_row + delta * pixel2grid_scale_row/cell_size_row)*0.5 ]; 174 | right_top = [max_pattern_pixel_coord(i,1) + (pixel2grid_scale_col + delta * pixel2grid_scale_col/cell_size_col)*0.5,... 175 | max_pattern_pixel_coord(i,2) - (pixel2grid_scale_row + delta * pixel2grid_scale_row/cell_size_row)*0.5 ]; 176 | right_down = [max_pattern_pixel_coord(i,1) + (pixel2grid_scale_col + delta * pixel2grid_scale_col/cell_size_col)*0.5,... 177 | max_pattern_pixel_coord(i,2) + (pixel2grid_scale_row + delta * pixel2grid_scale_row/cell_size_row)*0.5 ]; 178 | left_down = [max_pattern_pixel_coord(i,1) - (pixel2grid_scale_col + delta * pixel2grid_scale_col/cell_size_col)*0.5,... 179 | max_pattern_pixel_coord(i,2) + (pixel2grid_scale_row + delta * pixel2grid_scale_row/cell_size_row)*0.5 ]; 180 | X = [left_top(1), right_top(1), right_down(1), left_down(1)]; 181 | Y = [left_top(2), right_top(2), right_down(2), left_down(2)]; 182 | fill(X,Y,'g','LineStyle','none'); 183 | end 184 | 185 | plot(max_pattern_pixel_coord(:,1), max_pattern_pixel_coord(:,2),'o','MarkerSize',5,'MarkerFaceColor',[1.0 0.25 1.0]); 186 | plot(max_pattern_pixel_coord(:,1), max_pattern_pixel_coord(:,2),'k'); 187 | 188 | all_points = []; 189 | for ut = utVec % for each sampling point of the trajectory of the pattern 190 | p_ut = [1 ut ut^2 ut^3 ut^4 ut^5] * kBasisMat * max_pattern_grid_coord; %evaluate position 191 | temp_pixel_coord = [pixel2grid_scale_col/cell_size_col * p_ut(1), pixel2grid_scale_row/cell_size_row * p_ut(2)]; 192 | all_points = [all_points; temp_pixel_coord]; 193 | end 194 | plot(all_points(:,1), all_points(:,2),'r','LineWidth',1.5); 195 | title(['current inflation: ' num2str(delta)]); 196 | hold off; 197 | if(is_exceed) 198 | disp('Collision-free condition is violated'); 199 | else 200 | disp('All patterns are collision free!'); 201 | end 202 | 203 | pause(0.2) 204 | end 205 | --------------------------------------------------------------------------------