├── .gitignore ├── Codes For ICAPS2017 paper ├── fig3_firstOrderDynamics │ ├── Calculate_Ergodicity.m │ ├── SMC_Update.m │ ├── firstOrder_coverage_metric.png │ ├── main.m │ ├── repulsive_vectorFields.m │ ├── t_0_firstOrder.png │ ├── t_2point5_firstOrder.png │ └── t_5_firstOrder.png ├── fig4_mezic_implementation │ ├── Calculate_Ergodicity.m │ ├── Mezic_failure_to_avoid_obstacles_2point5_sec.png │ ├── Mezic_failure_to_avoid_obstacles_5_sec.png │ ├── SMC_Update.m │ ├── ergodic_metric_Mezic_failure_to_avoid_obstacles.png │ ├── main.m │ └── t_0_firstOrder.png ├── fig7_secondOrderDynamics │ ├── Calculate_Ergodicity.m │ ├── Ergodicity_metric_secondOrder_new.png │ ├── main_second_order.m │ ├── secondOrderSMC.m │ ├── t_0_secondOrder.png │ ├── t_15_secondOrder_new.png │ └── t_30_secondOrder_new.png ├── fig_9_nonuniformCoverage │ ├── Calculate_Ergodicity.m │ ├── SMC_Update.m │ ├── coverage metric.fig │ ├── end.fig │ ├── main.m │ ├── mid.fig │ ├── nonuniform coverage.mp4 │ ├── repulsive_vectorFields.m │ └── start.fig └── section4c_dynamicsObstacles │ ├── Calculate_Ergodicity.m │ ├── SMC_Update.m │ ├── main.m │ ├── metric.png │ └── repulsive_vectorFields.m ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | 49 | # Translations 50 | *.mo 51 | *.pot 52 | 53 | # Django stuff: 54 | *.log 55 | local_settings.py 56 | 57 | # Flask stuff: 58 | instance/ 59 | .webassets-cache 60 | 61 | # Scrapy stuff: 62 | .scrapy 63 | 64 | # Sphinx documentation 65 | docs/_build/ 66 | 67 | # PyBuilder 68 | target/ 69 | 70 | # Jupyter Notebook 71 | .ipynb_checkpoints 72 | 73 | # pyenv 74 | .python-version 75 | 76 | # celery beat schedule file 77 | celerybeat-schedule 78 | 79 | # SageMath parsed files 80 | *.sage.py 81 | 82 | # dotenv 83 | .env 84 | 85 | # virtualenv 86 | .venv 87 | venv/ 88 | ENV/ 89 | 90 | # Spyder project settings 91 | .spyderproject 92 | .spyproject 93 | 94 | # Rope project settings 95 | .ropeproject 96 | 97 | # mkdocs documentation 98 | /site 99 | 100 | # mypy 101 | .mypy_cache/ 102 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig3_firstOrderDynamics/Calculate_Ergodicity.m: -------------------------------------------------------------------------------- 1 | function [Ergodicity_Metric] = Calculate_Ergodicity(Ck, muk, DomainBounds) 2 | Lx = DomainBounds.xmax - DomainBounds.xmin; 3 | Ly = DomainBounds.ymax - DomainBounds.ymin; 4 | Nkx = size(muk, 1); 5 | Nky = size(muk, 2); 6 | Nagents = 1; 7 | 8 | Ergodicity_Metric=0; 9 | for iagent = 1:Nagents 10 | for kx = 0:Nkx-1 11 | for ky = 0:Nky-1 12 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^1.5); 13 | Ergodicity_Metric=Ergodicity_Metric+lambda_k*(abs(Ck(kx+1, ky+1)-muk(kx+1, ky+1)))^2; 14 | end 15 | end 16 | 17 | end 18 | % display('Ergodicity_Metric:') 19 | end 20 | 21 | 22 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig3_firstOrderDynamics/SMC_Update.m: -------------------------------------------------------------------------------- 1 | %%BY: Hadi Salman 2 | % OCT/09/2016 3 | function [posagents, Ck] = SMC_Update(posagents, Ck, muk, time, dt, DomainBounds, AgentSpeed, obstacles) 4 | 5 | Lx = DomainBounds.xmax - DomainBounds.xmin; 6 | Ly = DomainBounds.ymax - DomainBounds.ymin; 7 | xmin = DomainBounds.xmin; 8 | ymin = DomainBounds.ymin; 9 | 10 | Nkx = size(muk, 1); 11 | Nky = size(muk, 2); 12 | Nagents = size(posagents, 1); 13 | 14 | % Updating Fourier Coefficients of Coverage Distribution 15 | for iagent=1:Nagents 16 | xrel = posagents(iagent, 1) - xmin; 17 | yrel = posagents(iagent, 2) - ymin; 18 | for kx = 0:Nkx-1 19 | for ky = 0:Nky-1 20 | 21 | hk = Lx*Ly; 22 | if kx ~= 0 23 | hk = hk * 0.5; 24 | end 25 | if ky ~= 0 26 | hk = hk * 0.5; 27 | end 28 | hk = sqrt(hk); 29 | 30 | Ck(kx+1, ky+1) = Ck(kx+1, ky+1) + (1/hk)*cos(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly) * dt; 31 | end 32 | end 33 | end 34 | 35 | % Computing controls 36 | s=1.5; 37 | 38 | for iagent = 1:Nagents 39 | Bjx = 0.0; 40 | Bjy = 0.0; 41 | 42 | xrel = posagents(iagent, 1) - xmin; 43 | yrel = posagents(iagent, 2) - ymin; 44 | for kx = 0:Nkx-1 45 | for ky = 0:Nky-1 46 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^s); 47 | 48 | hk = Lx*Ly; 49 | if kx ~= 0 50 | hk = hk * 0.5; 51 | end 52 | if ky ~= 0 53 | hk = hk * 0.5; 54 | end 55 | hk = sqrt(hk); 56 | 57 | Bjx = Bjx + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-kx * pi/Lx) * sin(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly); 58 | Bjy = Bjy + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-ky * pi/Ly) * cos(kx * pi * xrel/Lx) * sin(ky * pi * yrel/Ly); 59 | end 60 | end 61 | 62 | Bjnorm = sqrt(Bjx*Bjx + Bjy*Bjy); 63 | 64 | %% OBSTACLE AVOIDANCE - Reference: Motion planning and collision avoidance using navigation vector fields 65 | 66 | Fox = 0; 67 | Foy = 0; 68 | product_sigma = 1; 69 | %account for other agents 70 | for other_agent=1:Nagents 71 | if other_agent ~= iagent 72 | ro = 0.01;%radius of obstacle 73 | re = 0.01;%safety margin 74 | r_robot= 0.00;% radius of robot 75 | 76 | % other agents positions 77 | xo = [posagents(other_agent,1)]; 78 | yo = [posagents(other_agent,2)]; 79 | 80 | %direction of vector fields 81 | p = ([Bjx,Bjy]./Bjnorm); 82 | 83 | % refer to the paper sec III 84 | smoothness = 0.05; 85 | r = ro + r_robot + re + smoothness; 86 | BF = ro^2 - r^2; 87 | BZ = ro^2 - (ro + r_robot +re)^2; 88 | 89 | dx = posagents(iagent, 1)-xo; 90 | dy = posagents(iagent, 2)-yo; 91 | 92 | B = ro^2 - dx^2 - dy^2; 93 | 94 | if(B <= BF || B > 0) 95 | sigma = 1; 96 | elseif (B > BF && B < BZ) 97 | % a = 2/(BZ- BF)^3; 98 | % b = -3*(BZ+BF)/(BZ-BF)^3; 99 | % c = 6*BZ*BF/(BZ-BF)^3; 100 | % d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 101 | % sigma =a*B^3 + b*B^2 + c*B + d; 102 | sigma =1-abs((B - BF)/(BF-BZ)); 103 | else 104 | sigma = 0; 105 | end 106 | product_sigma = product_sigma * sigma; 107 | if (p*[dx; dy] >= 0) 108 | Fox = Fox + (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 109 | Foy = Foy + (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 110 | end 111 | 112 | if (p*[dx; dy]<0) 113 | Fox = Fox + (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 114 | Foy = Foy +(1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 115 | end 116 | end 117 | end 118 | 119 | % account for obstacles 120 | for iobstacle = 1:obstacles.number 121 | 122 | ro = obstacles.r(iobstacle);%radius of obstacle 123 | re = 0.01;%safety margin 124 | r_robot= 0.00;% radius of robot 125 | 126 | % obstacles' positions 127 | xo = [obstacles.p(1,iobstacle)]; 128 | yo = [obstacles.p(2,iobstacle)]; 129 | 130 | %direction of vector fields 131 | p = ([Bjx,Bjy]./Bjnorm); 132 | 133 | % refer to the paper sec III 134 | smoothness = 0.05; 135 | r = ro + r_robot + re + smoothness; 136 | BF = ro^2 - r^2; 137 | BZ = ro^2 - (ro + r_robot +re)^2; 138 | 139 | dx = posagents(iagent, 1)-xo; 140 | dy = posagents(iagent, 2)-yo; 141 | 142 | B = ro^2 - dx^2 - dy^2; 143 | 144 | if(B <= BF || B > 0) 145 | sigma = 1; 146 | elseif (B > BF && B < BZ) 147 | % a = 2/(BZ- BF)^3; 148 | % b = -3*(BZ+BF)/(BZ-BF)^3; 149 | % c = 6*BZ*BF/(BZ-BF)^3; 150 | % d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 151 | % sigma =a*B^3 + b*B^2 + c*B + d; 152 | sigma =1-abs((B - BF)/(BF-BZ)); 153 | else 154 | sigma = 0; 155 | end 156 | product_sigma = product_sigma * sigma; 157 | if (p*[dx; dy] >= 0) 158 | Fox = Fox + (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 159 | Foy = Foy + (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 160 | end 161 | 162 | if (p*[dx; dy]<0) 163 | Fox = Fox + (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 164 | Foy = Foy +(1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 165 | end 166 | end 167 | Fox = Fox - product_sigma * Bjx/Bjnorm; 168 | Foy = Foy - product_sigma * Bjy/Bjnorm; 169 | L = sqrt(Fox^2 + Foy^2); 170 | 171 | % Updating agent location based on SMC feedback control law 172 | posagents(iagent,:) = posagents(iagent,:) + AgentSpeed * [Fox/L,Foy/L] * dt; 173 | 174 | % reflecting agent in case it goes out of domain bounds 175 | posagents(iagent, :) = reflect_agent(posagents(iagent, :),DomainBounds);%hadi no Need for this 176 | 177 | end 178 | 179 | end 180 | 181 | function [agent] = reflect_agent(agent, DomainBounds) 182 | 183 | xmin = DomainBounds.xmin; 184 | xmax = DomainBounds.xmax; 185 | ymin = DomainBounds.ymin; 186 | ymax = DomainBounds.ymax; 187 | 188 | if agent(1) < xmin 189 | agent(1) = xmin + (xmin - agent(1, 1)); 190 | end 191 | if agent(1) > xmax 192 | agent(1) = xmax - (agent(1, 1) - xmax); 193 | end 194 | if agent(2) < ymin 195 | agent(2) = ymin + (ymin - agent(1, 2)); 196 | end 197 | if agent(2) > ymax 198 | agent(2) = ymax - (agent(1, 2) - ymax); 199 | end 200 | 201 | end 202 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig3_firstOrderDynamics/firstOrder_coverage_metric.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig3_firstOrderDynamics/firstOrder_coverage_metric.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig3_firstOrderDynamics/main.m: -------------------------------------------------------------------------------- 1 | %% BY: Hadi Salman 2 | % 10/09/2016 3 | %% This codes prevents agent-agent collisions.(dynamic-obstacle avoidance) 4 | %%JUST CLICK RUN! MAKE SURE YOU DON'T START IN AN OBSTACLE REGION 5 | %% Setting domain bounds 6 | clear all; close all; 7 | DomainBounds.xmin = 0.0; 8 | DomainBounds.xmax = 1.0; 9 | DomainBounds.ymin = 0.0; 10 | DomainBounds.ymax = 1.0; 11 | Lx = DomainBounds.xmax - DomainBounds.xmin; 12 | Ly = DomainBounds.ymax - DomainBounds.ymin; 13 | 14 | %obstacles 15 | obstacles.r = [0.1,0.05,0.08,0.05];% radii of obstacles 16 | obstacles.p = [[0.5;0.5],[0.2;0.3],[0.7;0.2],[0.3;0.8]];%positions of obstacles[x1 y1 17 | % x2 y2 18 | % .....] 19 | obstacles.number = numel(obstacles.r); 20 | 21 | % Number of wave-numbers to be used 22 | Nk = 50;%% 23 | livePlot = true; %set if you want live plot for trajectories, other for faster execution 24 | %% Calculating muk 25 | res = 100;% resolution of discretization 26 | xdel=Lx/res; 27 | ydel=Ly/res; 28 | 29 | mu=ones(res,res); 30 | for xRange=0:xdel:Lx-xdel 31 | for yRange=0:ydel:Ly-ydel 32 | for i=1:obstacles.number 33 | if (xRange-obstacles.p(1,i))^2 + (yRange-obstacles.p(2,i))^2 <= obstacles.r(i)^2 34 | mu(uint8(xRange*res+1),uint8(yRange*res+1)) = 0; 35 | end 36 | end 37 | end 38 | end 39 | 40 | mu=mu./sum(sum(mu)); 41 | [X,Y]=meshgrid(1:res,1:res); 42 | 43 | muk=zeros(Nk,Nk); 44 | Nkx = size(muk, 1); 45 | Nky = size(muk, 2); 46 | for kx = 0:Nkx-1 47 | for ky = 0:Nky-1 48 | 49 | hk=Lx*Ly; %using lim x->0 sinx/x=1 50 | if kx ~= 0 51 | hk = hk * 0.5; 52 | end 53 | if ky ~= 0 54 | hk = hk * 0.5; 55 | end 56 | hk = sqrt(hk); 57 | 58 | for xRange=0:xdel:Lx-xdel 59 | for yRange=0:ydel:Ly-ydel 60 | muk(kx+1, ky+1) = muk(kx+1, ky+1)+ mu(uint8(xRange*res+1),uint8(yRange*res+1)) *(1/hk)*cos(kx * pi * xRange/Lx) * cos(ky * pi * yRange/Ly); 61 | 62 | end 63 | end 64 | 65 | 66 | end 67 | end 68 | 69 | %% Initializing agent locations 70 | Nagents = 4; 71 | posagents = [0.35,0.7;0.05,0.3;0.1,0.95;0.83,0.03];%make sure not to start in an obstacle region! 72 | AgentSpeed = 5; 73 | 74 | colors = ['m','g','b','c']; 75 | 76 | Nsteps = 100; 77 | dt = 0.001; 78 | 79 | % Initializing Fourier coefficients of coverage distribution 80 | Ck = zeros(Nk, Nk); 81 | 82 | figure(1); hold on; 83 | viscircles(obstacles.p',obstacles.r); 84 | for xRange=0:xdel:Lx-xdel 85 | for yRange=0:ydel:Ly-ydel 86 | for i=1:obstacles.number 87 | if (xRange-obstacles.p(1,i))^2 + (yRange-obstacles.p(2,i))^2 <= obstacles.r(i)^2 88 | scatter(xRange, yRange,2,'r','fill'); 89 | end 90 | end 91 | end 92 | end 93 | axis equal; 94 | 95 | 96 | ck_t = zeros(Nk, Nk); 97 | phi_squared = zeros(Nsteps,1); 98 | s= 1.5; 99 | 100 | 101 | % Executing multiple steps of SMC algorithm 102 | Ergodicity_Metric_save=0; 103 | for it = 1:Nsteps 104 | time = it * dt; 105 | [posagents, Ck] = SMC_Update(posagents, Ck, muk, time, dt, DomainBounds, AgentSpeed,obstacles); 106 | ck_t = Ck/(Nagents*time); 107 | for iagent = 1:Nagents 108 | plot(posagents(iagent, 1), posagents(iagent, 2), 'Color', colors(iagent) , 'Marker', 'o', 'MarkerSize', 1); 109 | if livePlot == true 110 | pause(0.001) 111 | end 112 | end 113 | 114 | if mod(it,100)==0 115 | fprintf('Iteration: %i/%i \n', it,Nsteps) 116 | end 117 | [Ergodicity_Metric] = Calculate_Ergodicity(Ck/Nagents/time, muk,DomainBounds); 118 | Ergodicity_Metric_save=[Ergodicity_Metric_save,Ergodicity_Metric]; 119 | 120 | end 121 | %% Plotting the metric of ergodicity 122 | time=0:0.001:0.001*(Nsteps); 123 | figure;loglog(time(2:end),Ergodicity_Metric_save(2:end)) 124 | axis([0.001 5 0.0001,1]) 125 | xlabel('Time'); 126 | ylabel('Coverage Metric, \phi(t)'); 127 | title('Metric of ergodicity as a function of time') 128 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig3_firstOrderDynamics/repulsive_vectorFields.m: -------------------------------------------------------------------------------- 1 | %%BY: Hadi Salman 2 | % OCT/05/2016 3 | %% Reference: Motion planning and collision avoidance using navigation vector fields 4 | % JUST CLICK RUN! 5 | 6 | %% 7 | 8 | ro = 1;%radius of obstacle 9 | re = 0.0;%safety margin 10 | r_robot= 0.0;% radius of robot 11 | 12 | figure; 13 | hold on 14 | viscircles([-2,-2],ro); 15 | axis equal; 16 | axis([-5 2 -5 2]); 17 | % scatter(0,0,'x'); 18 | % text(0,0,' target'); 19 | 20 | % obstacles' positions 21 | xo = [-2]; 22 | yo = [-2]; 23 | N = numel(xo); % number of obstacles 24 | 25 | % repulsive vector field (for one obstacle) 26 | phi = atan2(- yo, -xo)+ pi; 27 | p = [cos(phi), sin(phi)];% direction from obstacle to origin 28 | % p = [1,0]; 29 | resolution = 0.3; 30 | [x,y] = meshgrid(-5.0:resolution:2.0,-5.0:resolution:2.0); 31 | 32 | % %repalsive vector fields around obstacle o 33 | % %lambda = 1: p.deltaR > 0 (Check ref. paper sec: III) 34 | % Fox1 = p(2)*(x-xo).*(y-yo) - p(1)*(y-yo).^2; 35 | % Foy1 = p(1)*(x-xo).*(y-yo) - p(2)*(x-xo).^2; 36 | % 37 | % %lambda = 0 p.deltaR < 0 (Check ref. paper sec: III) 38 | % Fox0 = -p(1)*(x-xo).^2 - p(1)*(y-yo).^2; 39 | % Foy0 = -p(2)*(y-yo).^2 - p(2)*(x-xo).^2; 40 | 41 | %% 42 | % refer to the paper sec III 43 | %valur of B at some point r > rz = ro+re+r 44 | r = ro + r_robot + re + 1; 45 | BF = ro^2 - r^2; 46 | BZ = ro^2 - (ro + r_robot +re)^2; 47 | 48 | i=0; 49 | j=0; 50 | Fox = zeros(size(x)); 51 | Foy = zeros(size(x)); 52 | for xi = -5.0:resolution:2.0 53 | j=j+1; 54 | i=0; 55 | for yi = -5.0:resolution:2.0 56 | i=i+1; 57 | dx = xi-xo; 58 | dy = yi-yo; 59 | 60 | B = ro^2 - dx^2 - dy^2; 61 | 62 | if(B <= BF || B > 0) 63 | sigma = 1; 64 | elseif (B > BF && B < BZ) 65 | a = 2/(BZ- BF)^3; 66 | b = -3*(BZ+BF)/(BZ-BF)^3; 67 | c = 6*BZ*BF/(BZ-BF)^3; 68 | d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 69 | sigma =a*B^3 + b*B^2 + c*B + d; 70 | else 71 | sigma = 0; 72 | end 73 | 74 | if (p*[dx; dy] >= 0) 75 | Fox(i, j) = (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([(p(2)*(dx)*(dy) - p(1)*(dy)^2),(p(1)*(dx)*(dy) - p(2)*(dx)^2)]); 76 | Foy(i, j) = (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([(p(2)*(dx)*(dy) - p(1)*(dy)^2),(p(1)*(dx)*(dy) - p(2)*(dx)^2)]); 77 | end 78 | 79 | if (p*[dx; dy]<0) 80 | Fox(i, j) = (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([(-p(1)*(dx).^2 - p(1)*(dy).^2),(-p(2)*(dy).^2 - p(2)*(dx).^2)]); 81 | Foy(i, j) = (1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([(-p(1)*(dx).^2 - p(1)*(dy).^2),(-p(2)*(dy).^2 - p(2)*(dx).^2)]); 82 | end 83 | end 84 | end 85 | 86 | % Fox = Fox1; 87 | % Foy = Foy1; 88 | % figure(1) 89 | L = sqrt(Fox.^2 + Foy.^2); 90 | quiver(x,y,Fox./L,Foy./L); 91 | 92 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig3_firstOrderDynamics/t_0_firstOrder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig3_firstOrderDynamics/t_0_firstOrder.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig3_firstOrderDynamics/t_2point5_firstOrder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig3_firstOrderDynamics/t_2point5_firstOrder.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig3_firstOrderDynamics/t_5_firstOrder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig3_firstOrderDynamics/t_5_firstOrder.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig4_mezic_implementation/Calculate_Ergodicity.m: -------------------------------------------------------------------------------- 1 | function [Ergodicity_Metric] = Calculate_Ergodicity(Ck, muk, DomainBounds) 2 | Lx = DomainBounds.xmax - DomainBounds.xmin; 3 | Ly = DomainBounds.ymax - DomainBounds.ymin; 4 | Nkx = size(muk, 1); 5 | Nky = size(muk, 2); 6 | 7 | 8 | Ergodicity_Metric=0; 9 | for kx = 0:Nkx-1 10 | for ky = 0:Nky-1 11 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^1.5); 12 | Ergodicity_Metric=Ergodicity_Metric+lambda_k*(abs(Ck(kx+1, ky+1)-muk(kx+1, ky+1)))^2; 13 | end 14 | end 15 | % display('Ergodicity_Metric:') 16 | end 17 | 18 | 19 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig4_mezic_implementation/Mezic_failure_to_avoid_obstacles_2point5_sec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig4_mezic_implementation/Mezic_failure_to_avoid_obstacles_2point5_sec.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig4_mezic_implementation/Mezic_failure_to_avoid_obstacles_5_sec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig4_mezic_implementation/Mezic_failure_to_avoid_obstacles_5_sec.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig4_mezic_implementation/SMC_Update.m: -------------------------------------------------------------------------------- 1 | function [posagents, Ck] = SMC_Update(posagents, Ck, muk, time, dt, DomainBounds, AgentSpeed) 2 | 3 | Lx = DomainBounds.xmax - DomainBounds.xmin; 4 | Ly = DomainBounds.ymax - DomainBounds.ymin; 5 | xmin = DomainBounds.xmin; 6 | ymin = DomainBounds.ymin; 7 | 8 | Nkx = size(muk, 1); 9 | Nky = size(muk, 2); 10 | Nagents = size(posagents, 1); 11 | 12 | % Updating Fourier Coefficients of Coverage Distribution 13 | for iagent=1:Nagents 14 | xrel = posagents(iagent, 1) - xmin; 15 | yrel = posagents(iagent, 2) - ymin; 16 | for kx = 0:Nkx-1 17 | for ky = 0:Nky-1 18 | 19 | % hk = sqrt(Lx*Ly); %hadi 20 | hk = Lx*Ly; %hadi 21 | if kx ~= 0 22 | hk = hk * 0.5; 23 | end 24 | if ky ~= 0 25 | hk = hk * 0.5; 26 | end 27 | hk = sqrt(hk);%hadi 28 | 29 | Ck(kx+1, ky+1) = Ck(kx+1, ky+1) + (1/hk)*cos(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly) * dt; 30 | end 31 | end 32 | end 33 | 34 | % Computing controls 35 | s=1.5; 36 | %Ergodicity_Metric=0; 37 | for iagent = 1:Nagents 38 | Bjx = 0.0; %hadi should be inside loop not outside 39 | Bjy = 0.0; %hadi 40 | 41 | xrel = posagents(iagent, 1) - xmin; 42 | yrel = posagents(iagent, 2) - ymin; 43 | for kx = 0:Nkx-1 44 | for ky = 0:Nky-1 45 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^s); 46 | % hk = sqrt(Lx*Ly); %hadi 47 | hk = Lx*Ly; %hadi 48 | if kx ~= 0 49 | hk = hk * 0.5; 50 | end 51 | if ky ~= 0 52 | hk = hk * 0.5; 53 | end 54 | hk = sqrt(hk); %hadi 55 | 56 | Bjx = Bjx + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-kx * pi/Lx) * sin(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly); 57 | Bjy = Bjy + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-ky * pi/Ly) * cos(kx * pi * xrel/Lx) * sin(ky * pi * yrel/Ly); 58 | end 59 | end 60 | 61 | Bjnorm = sqrt(Bjx*Bjx + Bjy*Bjy); 62 | 63 | % Updating agent location based on SMC feedback control law 64 | posagents(iagent,:) = posagents(iagent,:) - AgentSpeed * ([Bjx,Bjy]./Bjnorm) * dt; 65 | 66 | %No Need for this 67 | % reflecting agent in case it goes out of domain bounds 68 | posagents(iagent, :) = reflect_agent(posagents(iagent, :), DomainBounds); 69 | end 70 | 71 | end 72 | 73 | function [agent] = reflect_agent(agent, DomainBounds) 74 | 75 | xmin = DomainBounds.xmin; 76 | xmax = DomainBounds.xmax; 77 | ymin = DomainBounds.ymin; 78 | ymax = DomainBounds.ymax; 79 | 80 | if agent(1) < xmin 81 | agent(1) = xmin + (xmin - agent(1, 1)); 82 | end 83 | if agent(1) > xmax 84 | agent(1) = xmax - (agent(1, 1) - xmax); 85 | end 86 | if agent(2) < ymin 87 | agent(2) = ymin + (ymin - agent(1, 2)); 88 | end 89 | if agent(2) > ymax 90 | agent(2) = ymax - (agent(1, 2) - ymax); 91 | end 92 | 93 | end 94 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig4_mezic_implementation/ergodic_metric_Mezic_failure_to_avoid_obstacles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig4_mezic_implementation/ergodic_metric_Mezic_failure_to_avoid_obstacles.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig4_mezic_implementation/main.m: -------------------------------------------------------------------------------- 1 | %% Setting domain bounds 2 | DomainBounds.xmin = 0.0; 3 | DomainBounds.xmax = 1.0; 4 | DomainBounds.ymin = 0.0; 5 | DomainBounds.ymax = 1.0; 6 | Lx = DomainBounds.xmax - DomainBounds.xmin; 7 | Ly = DomainBounds.ymax - DomainBounds.ymin; 8 | 9 | %obstacles 10 | obstacles.r = [0.1,0.05,0.08,0.05];% radii of obstacles 11 | obstacles.p = [[0.5;0.5],[0.2;0.3],[0.7;0.2],[0.3;0.8]];%positions of obstacles[x1 y1 12 | % x2 y2 13 | .....] 14 | obstacles.number = numel(obstacles.r); 15 | 16 | % Number of wave-numbers to be used 17 | Nk = 50;%% 18 | livePlot = true; %set if you want live plot for trajectories, other for faster execution 19 | %% Calculating muk 20 | res = 100;% resolution of discretization 21 | mu=ones(res,res); 22 | xdel=Lx/res; 23 | ydel=Ly/res; 24 | 25 | % for xRange=0:xdel:Lx-xdel 26 | % for yRange=0:ydel:Ly-ydel 27 | % for i=1:obstacles.number 28 | % if (xRange-obstacles.p(1,i))^2 + (yRange-obstacles.p(2,i))^2 <= obstacles.r(i)^2 29 | % mu(uint8(xRange*res+1),uint8(yRange*res+1)) = 0; 30 | % end 31 | % end 32 | % end 33 | % end 34 | mu=mu./sum(sum(mu)); 35 | [X,Y]=meshgrid(1:res,1:res); 36 | surf(X,Y,mu); 37 | close; 38 | % muk = dct2(mu,Nk,Nk); 39 | %For Matlab DCT to work: (k1,k2)=1,2,..(L1-L2) 40 | 41 | muk=zeros(Nk,Nk); 42 | xdel=Lx/res; 43 | ydel=Ly/res; 44 | Nkx = size(muk, 1); 45 | Nky = size(muk, 2); 46 | for kx = 0:Nkx-1 47 | for ky = 0:Nky-1 48 | 49 | hk=Lx*Ly; %using lim x->0 sinx/x=1 50 | if kx ~= 0 51 | hk = hk * 0.5; 52 | end 53 | if ky ~= 0 54 | hk = hk * 0.5; 55 | end 56 | hk = sqrt(hk); 57 | 58 | for xRange=0:xdel:Lx-xdel 59 | for yRange=0:ydel:Ly-ydel 60 | muk(kx+1, ky+1) = muk(kx+1, ky+1)+ mu(uint8(xRange*res+1),uint8(yRange*res+1)) *(1/hk)*cos(kx * pi * xRange/Lx) * cos(ky * pi * yRange/Ly); 61 | 62 | end 63 | end 64 | 65 | 66 | end 67 | end 68 | 69 | %% Initializing agent locations 70 | Nagents = 4; 71 | posagents = [0.35,0.7;0.05,0.3;0.1,0.95;0.83,0.03];%make sure not to start in an obstacle region! 72 | AgentSpeed = 5; 73 | 74 | colors = ['m','g','b','c']; 75 | 76 | Nsteps = 5000; 77 | dt = 0.001; 78 | 79 | % Initializing Fourier coefficients of coverage distribution 80 | Ck = zeros(Nk, Nk); 81 | ck_t = zeros(Nk, Nk); 82 | figure(1); hold on; 83 | viscircles(obstacles.p',obstacles.r); 84 | %color inside the circles 85 | for xRange=0:xdel:Lx-xdel 86 | for yRange=0:ydel:Ly-ydel 87 | for i=1:obstacles.number 88 | if (xRange-obstacles.p(1,i))^2 + (yRange-obstacles.p(2,i))^2 <= obstacles.r(i)^2 89 | scatter(xRange, yRange,2,'r','fill'); 90 | end 91 | end 92 | end 93 | end 94 | 95 | axis equal; 96 | 97 | phi_squared = zeros(Nsteps,1); 98 | s= 1.5; 99 | % Executing multiple steps of SMC algorithm 100 | Ergodicity_Metric_save=0; 101 | tic 102 | for it = 1:Nsteps 103 | time = (it) * dt; 104 | [posagents, Ck] = SMC_Update(posagents, Ck, muk, time, dt, DomainBounds, AgentSpeed); 105 | ck_t = Ck/(Nagents*time); 106 | for iagent = 1:Nagents 107 | plot(posagents(iagent, 1), posagents(iagent, 2), 'Color', colors(iagent), 'Marker', 'o', 'MarkerSize', 1); 108 | if livePlot == true 109 | pause(0.001) 110 | end 111 | end 112 | if mod(it,100)==0 113 | fprintf('Iteration: %i/%i \n', it,Nsteps) 114 | end 115 | 116 | [Ergodicity_Metric] = Calculate_Ergodicity(Ck/Nagents/time, muk,DomainBounds); 117 | Ergodicity_Metric_save=[Ergodicity_Metric_save,Ergodicity_Metric]; 118 | 119 | end 120 | toc 121 | %% Plotting the metric of ergodicity 122 | time=0:0.001:0.001*Nsteps; 123 | figure;loglog(time(2:end),Ergodicity_Metric_save(2:end)) 124 | axis([0.001 5 0.0001,1]) 125 | xlabel('Time'); 126 | ylabel('Coverage Metric, \phi(t)'); 127 | title('Metric of ergodicity as a function of time') 128 | 129 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig4_mezic_implementation/t_0_firstOrder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig4_mezic_implementation/t_0_firstOrder.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig7_secondOrderDynamics/Calculate_Ergodicity.m: -------------------------------------------------------------------------------- 1 | function [Ergodicity_Metric] = Calculate_Ergodicity(Ck, muk, DomainBounds) 2 | Lx = DomainBounds.xmax - DomainBounds.xmin; 3 | Ly = DomainBounds.ymax - DomainBounds.ymin; 4 | Nkx = size(muk, 1); 5 | Nky = size(muk, 2); 6 | Nagents = 1; 7 | 8 | Ergodicity_Metric=0; 9 | for iagent = 1:Nagents 10 | for kx = 0:Nkx-1 11 | for ky = 0:Nky-1 12 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^1.5); 13 | Ergodicity_Metric=Ergodicity_Metric+lambda_k*(abs(Ck(kx+1, ky+1)-muk(kx+1, ky+1)))^2; 14 | end 15 | end 16 | 17 | end 18 | % display('Ergodicity_Metric:') 19 | end 20 | 21 | 22 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig7_secondOrderDynamics/Ergodicity_metric_secondOrder_new.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig7_secondOrderDynamics/Ergodicity_metric_secondOrder_new.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig7_secondOrderDynamics/main_second_order.m: -------------------------------------------------------------------------------- 1 | %% By: Hadi Salman 2 | % 11/06/2016 3 | clear all; close all; 4 | global flag2 p limit 5 | limit =[]; 6 | flag2= [0,0,0,0];% to know when to change p 7 | p=ones(4,2); 8 | 9 | % Setting domain bounds 10 | DomainBounds.xmin = 0.0; 11 | DomainBounds.xmax = 1.0; 12 | DomainBounds.ymin = 0.0; 13 | DomainBounds.ymax = 1.0; 14 | Lx = DomainBounds.xmax - DomainBounds.xmin; 15 | Ly = DomainBounds.ymax - DomainBounds.ymin; 16 | 17 | %obstacles 18 | obstacles.r = [0.1,0.05,0.08,0.05];% radii of obstacles 19 | obstacles.p = [[0.5;0.5],[0.2;0.3],[0.7;0.2],[0.3;0.8]];%positions of obstacles[x1 y1 20 | % x2 y2 21 | % .....] 22 | obstacles.number = numel(obstacles.r); 23 | 24 | % Number of wave-numbers to be used 25 | Nk = 50;%% 26 | livePlot = false; %set if you want live plot for trajectories, other for faster execution 27 | %% Calculating muk 28 | res = 100;% resolution of discretization 29 | xdel=Lx/res; 30 | ydel=Ly/res; 31 | 32 | mu=ones(res,res); 33 | for xRange=0:xdel:Lx-xdel 34 | for yRange=0:ydel:Ly-ydel 35 | for i=1:obstacles.number 36 | if (xRange-obstacles.p(1,i))^2 + (yRange-obstacles.p(2,i))^2 <= obstacles.r(i)^2 37 | mu(uint8(xRange*res+1),uint8(yRange*res+1)) = 0; 38 | end 39 | end 40 | end 41 | end 42 | 43 | mu=mu./sum(sum(mu)); 44 | [X,Y]=meshgrid(1:res,1:res); 45 | 46 | muk=zeros(Nk,Nk); 47 | Nkx = size(muk, 1); 48 | Nky = size(muk, 2); 49 | for kx = 0:Nkx-1 50 | for ky = 0:Nky-1 51 | 52 | hk=Lx*Ly; %using lim x->0 sinx/x=1 53 | if kx ~= 0 54 | hk = hk * 0.5; 55 | end 56 | if ky ~= 0 57 | hk = hk * 0.5; 58 | end 59 | hk = sqrt(hk); 60 | 61 | for xRange=0:xdel:Lx-xdel 62 | for yRange=0:ydel:Ly-ydel 63 | muk(kx+1, ky+1) = muk(kx+1, ky+1)+ mu(uint8(xRange*res+1),uint8(yRange*res+1)) *(1/hk)*cos(kx * pi * xRange/Lx) * cos(ky * pi * yRange/Ly); 64 | 65 | end 66 | end 67 | 68 | 69 | end 70 | end 71 | 72 | 73 | 74 | %% Initializing agent locations and velocities 75 | Nagents = 4; 76 | posagents = [0.35,0.7;0.05,0.3;0.1,0.95;0.83,0.03];%make sure not to start in an obstacle region! 77 | velagents =zeros(Nagents,2); 78 | AgentForce = 100.0; % max Force 79 | c = 2.5; % weighting parameter (how much kinetic energy is penalized) 80 | 81 | colors = ['m','g','b','c']; 82 | 83 | Nsteps = 6000; 84 | dt = 0.005; 85 | 86 | % Initializing Fourier coefficients of coverage distribution 87 | Ck = zeros(Nk, Nk); 88 | 89 | figure(1); hold on; 90 | viscircles(obstacles.p',obstacles.r); 91 | for xRange=0:xdel:Lx-xdel 92 | for yRange=0:ydel:Ly-ydel 93 | for i=1:obstacles.number 94 | if (xRange-obstacles.p(1,i))^2 + (yRange-obstacles.p(2,i))^2 <= obstacles.r(i)^2 95 | scatter(xRange, yRange,2,'r','fill'); 96 | end 97 | end 98 | end 99 | end 100 | 101 | 102 | axis equal; 103 | Fox_old = [0,0,0,0]; 104 | Foy_old = [0,0,0,0]; 105 | Ergodicity_Metric_save=0; 106 | % Executing multiple steps of SMC algorithm 107 | for it = 1:Nsteps 108 | if(it==3000) 109 | pause 110 | end 111 | time = it * dt; 112 | [posagents, velagents, Ck ,Fox_old, Foy_old] = secondOrderSMC(posagents, velagents, Ck, muk, time, dt, DomainBounds, AgentForce, c, obstacles, Fox_old, Foy_old); 113 | for iagent = 1:Nagents 114 | plot(posagents(iagent, 1), posagents(iagent, 2), 'Color', colors(iagent), 'Marker', 'o', 'MarkerSize', 1); 115 | if livePlot == true 116 | pause(0.001) 117 | end 118 | end 119 | if mod(it,100)==0 120 | fprintf('Iteration: %i/%i \n', it,Nsteps) 121 | end 122 | 123 | [Ergodicity_Metric] = Calculate_Ergodicity(Ck/Nagents/time, muk,DomainBounds); 124 | Ergodicity_Metric_save=[Ergodicity_Metric_save, Ergodicity_Metric]; 125 | 126 | end 127 | 128 | %% Plotting the metric of ergodicity 129 | time=0:0.005:0.005*(Nsteps); 130 | figure;loglog(time(2:end),Ergodicity_Metric_save(2:end)) 131 | axis([0.005 15 0.0001,1]) 132 | xlabel('Time'); 133 | ylabel('Coverage Metric, \phi(t)'); 134 | title('Metric of ergodicity as a function of time') 135 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig7_secondOrderDynamics/secondOrderSMC.m: -------------------------------------------------------------------------------- 1 | function [posagents, velagents, Ck,Fox_old, Foy_old] = secondOrderSMC(posagents, velagents, Ck, muk, time, dt, DomainBounds, AgentForce, c, obstacles,Fox_old, Foy_old) 2 | %% By: Hadi Salman 3 | % 11/06/2016 4 | global flag2 p limit 5 | Lx = DomainBounds.xmax - DomainBounds.xmin; 6 | Ly = DomainBounds.ymax - DomainBounds.ymin; 7 | xmin = DomainBounds.xmin; 8 | ymin = DomainBounds.ymin; 9 | 10 | Nkx = size(muk, 1); 11 | Nky = size(muk, 2); 12 | Nagents = size(posagents, 1); 13 | 14 | %% Updating Fourier Coefficients of Coverage Distribution 15 | for iagent=1:Nagents 16 | xrel = posagents(iagent, 1) - xmin; 17 | yrel = posagents(iagent, 2) - ymin; 18 | for kx = 0:Nkx-1 19 | for ky = 0:Nky-1 20 | 21 | % hk = sqrt(Lx*Ly); % hadi 22 | hk = Lx*Ly; %hadi 23 | if kx ~= 0 24 | hk = hk * 0.5; 25 | end 26 | if ky ~= 0 27 | hk = hk * 0.5; 28 | end 29 | hk = sqrt(hk);%hadi 30 | 31 | Ck(kx+1, ky+1) = Ck(kx+1, ky+1) + (1/hk)*cos(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly) * dt; 32 | end 33 | end 34 | end 35 | 36 | %% Computing controls 37 | s=1.5; 38 | 39 | 40 | %Ergodicity_Metric=0; 41 | for iagent = 1:Nagents 42 | 43 | Bjx = 0.0; 44 | Bjy = 0.0; 45 | xrel = posagents(iagent, 1) - xmin; 46 | yrel = posagents(iagent, 2) - ymin; 47 | for kx = 0:Nkx-1 48 | for ky = 0:Nky-1 49 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^s); 50 | % hk = sqrt(Lx*Ly); %hadi 51 | hk = Lx*Ly; %hadi 52 | if kx ~= 0 53 | hk = hk * 0.5; 54 | end 55 | if ky ~= 0 56 | hk = hk * 0.5; 57 | end 58 | hk = sqrt(hk); %hadi 59 | 60 | Bjx = Bjx + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-kx * pi/Lx) * sin(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly); 61 | Bjy = Bjy + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-ky * pi/Ly) * cos(kx * pi * xrel/Lx) * sin(ky * pi * yrel/Ly); 62 | end 63 | end 64 | 65 | Bjnorm = sqrt(Bjx*Bjx + Bjy*Bjy); 66 | 67 | 68 | %% OBSTACLE AVOIDANCE - Reference: Motion planning and collision avoidance using navigation vector fields 69 | Fox = 0; 70 | Foy = 0; 71 | flag = 0; 72 | 73 | if (flag2(iagent) == 0)%not changing as soon as it becomes in the vicinity of an obstacle 74 | p(iagent,:) = ([Bjx,Bjy]./Bjnorm); 75 | % p(iagent,:) = - velagents(iagent, :)/(norm( velagents(iagent, :),2)) 76 | % p(iagent,:) = (c*velagents (iagent,:) + [Bjx, Bjy])/norm(c*velagents (iagent,:) + [Bjx, Bjy]); 77 | end 78 | 79 | flag2(iagent) = 0; 80 | for iobstacle = 1:obstacles.number 81 | ro = obstacles.r(iobstacle);%radius of obstacle 82 | re = 0.01;%safety margin 83 | r_robot= 0.00;% radius of robot 84 | 85 | % obstacles' positions 86 | xo = [obstacles.p(1,iobstacle)]; 87 | yo = [obstacles.p(2,iobstacle)]; 88 | 89 | 90 | 91 | % refer to the paper sec III 92 | %valur of B at some point r > rz = ro+re+r 93 | safety = 0.05; 94 | r = ro + r_robot + re + safety; 95 | BF = ro^2 - r^2; 96 | BZ = ro^2 - (ro + r_robot +re)^2; 97 | 98 | dx = posagents(iagent, 1)-xo; 99 | dy = posagents(iagent, 2)-yo; 100 | 101 | B = ro^2 - dx^2 - dy^2; 102 | 103 | if(B <= BF || B > 0) 104 | sigma = 1;% in vicinity of an obstacles 105 | flag2(iagent) = flag2(iagent) | 0; 106 | elseif (B > BF && B < BZ) 107 | flag2(iagent) = 1; 108 | flag = 1;%in vicinity of obstacle 109 | % a = 2/(BZ- BF)^3; 110 | % b = -3*(BZ+BF)/(BZ-BF)^3; 111 | % c = 6*BZ*BF/(BZ-BF)^3; 112 | % d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 113 | % sigma =a*B^3 + b*B^2 + c*B + d; 114 | sigma = 1 - abs((B - BF)/(BF-BZ)); 115 | else 116 | flag2(iagent) = 1; 117 | flag = 1;%in vicinity of obstacle 118 | sigma = 0; 119 | end 120 | if(flag == 1) 121 | if (p(iagent,:)*[dx; dy] >= 0) 122 | % Fox = (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]) - sigma*((c*velagents(iagent,1) + Bjx)./norm(c*velagents (iagent,1) + Bjx)); 123 | % Foy = (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]) - sigma*((c*velagents(iagent,2) + Bjy)./norm(c*velagents (iagent,2) + Bjy)); 124 | Fox = Fox + (1-sigma)*(p(iagent,2)*(dx)*(dy) - p(iagent,1)*(dy)^2)/norm([p(iagent,2)*(dx)*(dy) - p(iagent,1)*(dy)^2,p(iagent,1)*(dx)*(dy) - p(2)*(dx)^2]) ; 125 | Foy = Foy + (1-sigma)*(p(iagent,1)*(dx)*(dy) - p(iagent,2)*(dx)^2)/norm([p(iagent,2)*(dx)*(dy) - p(iagent,1)*(dy)^2,p(iagent,1)*(dx)*(dy) - p(iagent,2)*(dx)^2]) ; 126 | 127 | end 128 | 129 | if (p(iagent,:)*[dx; dy]<0) 130 | % Fox = (1-sigma)*(-p(iagent,1)*(dx).^2 - p(iagent,1)*(dy).^2)/norm([-p(iagent,1)*(dx).^2 - p(iagent,1)*(dy).^2,-p(iagent,2)*(dy).^2 - p(iagent,2)*(dx).^2])- sigma*((c*velagents(iagent,1) + Bjx)./norm(c*velagents (iagent,1) + Bjx)); 131 | % Foy = (1-sigma)*(-p(iagent,2)*(dy).^2 - p(iagent,2)*(dx).^2)/norm([-p(iagent,1)*(dx).^2 - p(iagent,1)*(dy).^2,-p(iagent,2)*(dy).^2 - p(iagent,2)*(dx).^2])- sigma*((c*velagents(iagent,2) + Bjy)./norm(c*velagents (iagent,2) + Bjy)); 132 | Fox = Fox + (1-sigma)*(-p(iagent,1)*(dx).^2 - p(iagent,1)*(dy).^2)/norm([-p(iagent,1)*(dx).^2 - p(iagent,1)*(dy).^2,-p(iagent,2)*(dy).^2 - p(iagent,2)*(dx).^2]); 133 | Foy = Foy +(1-sigma)*(-p(iagent,2)*(dy).^2 - p(iagent,2)*(dx).^2)/norm([-p(iagent,1)*(dx).^2 - p(iagent,1)*(dy).^2,-p(iagent,2)*(dy).^2 - p(iagent,2)*(dx).^2]); 134 | end 135 | end 136 | end 137 | if flag==1 138 | Fxdot = (Fox - Fox_old(iagent))/dt; 139 | Fydot = (Foy - Foy_old(iagent))/dt; 140 | Fox_old(iagent) = Fox; 141 | Foy_old(iagent) = Foy; 142 | 143 | L = sqrt(Fox^2 + Foy^2); 144 | end 145 | % Updating agent location based on SMC feedback control law 146 | % posagents(iagent,:) = posagents(iagent,:) + AgentSpeed * [Fox/L,Foy/L] * dt; 147 | k=30; 148 | 149 | %% Updating agent location based on SMC feedback control law 150 | posagents(iagent,:) = posagents(iagent, :) + velagents (iagent,:) * dt; 151 | if(flag == 1) 152 | velagents (iagent,:) = (velagents (iagent,:) + (k*(0.35 * [Fox/L,Foy/L] - velagents (iagent,:)) + .35*[Fxdot,Fydot])* dt); 153 | limit = [limit norm((k*(.35 * [Fox/L,Foy/L] - velagents (iagent,:)) + .35*[Fxdot,Fydot]))]; 154 | else 155 | velagents (iagent,:) = (velagents (iagent,:) - AgentForce * ((c*velagents (iagent,:) + [Bjx, Bjy])./norm(c*velagents (iagent,:) + [Bjx, Bjy]))* dt); 156 | end 157 | 158 | % Force the agent inward toward the domain with maximum force whenever they 159 | % leave the domain 160 | velagents(iagent, :) = forcingInward(posagents(iagent,:), velagents(iagent, :), DomainBounds, AgentForce, dt); 161 | end 162 | 163 | end 164 | %% 165 | function [velagent] = forcingInward(posagent, velagent, DomainBounds, AgentForce, dt) 166 | 167 | xmin = DomainBounds.xmin; 168 | xmax = DomainBounds.xmax; 169 | ymin = DomainBounds.ymin; 170 | ymax = DomainBounds.ymax; 171 | 172 | if posagent(1) < xmin 173 | velagent(1) = velagent(1) + AgentForce*dt; 174 | end 175 | if posagent(1) > xmax 176 | velagent(1) = velagent(1) - AgentForce*dt; 177 | end 178 | if posagent(2) < ymin 179 | velagent(2) = velagent(2) + AgentForce*dt; 180 | end 181 | if posagent(2) > ymax 182 | velagent(2) = velagent(2) - AgentForce*dt; 183 | end 184 | 185 | end 186 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig7_secondOrderDynamics/t_0_secondOrder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig7_secondOrderDynamics/t_0_secondOrder.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig7_secondOrderDynamics/t_15_secondOrder_new.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig7_secondOrderDynamics/t_15_secondOrder_new.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig7_secondOrderDynamics/t_30_secondOrder_new.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig7_secondOrderDynamics/t_30_secondOrder_new.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/Calculate_Ergodicity.m: -------------------------------------------------------------------------------- 1 | function [Ergodicity_Metric] = Calculate_Ergodicity(Ck, muk, DomainBounds) 2 | Lx = DomainBounds.xmax - DomainBounds.xmin; 3 | Ly = DomainBounds.ymax - DomainBounds.ymin; 4 | Nkx = size(muk, 1); 5 | Nky = size(muk, 2); 6 | Nagents = 1; 7 | 8 | Ergodicity_Metric=0; 9 | for iagent = 1:Nagents 10 | for kx = 0:Nkx-1 11 | for ky = 0:Nky-1 12 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^1.5); 13 | Ergodicity_Metric=Ergodicity_Metric+lambda_k*(abs(Ck(kx+1, ky+1)-muk(kx+1, ky+1)))^2; 14 | end 15 | end 16 | 17 | end 18 | % display('Ergodicity_Metric:') 19 | end 20 | 21 | 22 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/SMC_Update.m: -------------------------------------------------------------------------------- 1 | %%BY: Hadi Salman 2 | % OCT/09/2016 3 | function [posagents, Ck] = SMC_Update(posagents, Ck, muk, time, dt, DomainBounds, AgentSpeed, obstacles) 4 | 5 | Lx = DomainBounds.xmax - DomainBounds.xmin; 6 | Ly = DomainBounds.ymax - DomainBounds.ymin; 7 | xmin = DomainBounds.xmin; 8 | ymin = DomainBounds.ymin; 9 | 10 | Nkx = size(muk, 1); 11 | Nky = size(muk, 2); 12 | Nagents = size(posagents, 1); 13 | 14 | % Updating Fourier Coefficients of Coverage Distribution 15 | for iagent=1:Nagents 16 | xrel = posagents(iagent, 1) - xmin; 17 | yrel = posagents(iagent, 2) - ymin; 18 | for kx = 0:Nkx-1 19 | for ky = 0:Nky-1 20 | 21 | hk = Lx*Ly; 22 | if kx ~= 0 23 | hk = hk * 0.5; 24 | end 25 | if ky ~= 0 26 | hk = hk * 0.5; 27 | end 28 | hk = sqrt(hk); 29 | 30 | Ck(kx+1, ky+1) = Ck(kx+1, ky+1) + (1/hk)*cos(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly) * dt; 31 | end 32 | end 33 | end 34 | 35 | % Computing controls 36 | s=1.5; 37 | 38 | for iagent = 1:Nagents 39 | Bjx = 0.0; 40 | Bjy = 0.0; 41 | 42 | xrel = posagents(iagent, 1) - xmin; 43 | yrel = posagents(iagent, 2) - ymin; 44 | for kx = 0:Nkx-1 45 | for ky = 0:Nky-1 46 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^s); 47 | 48 | hk = Lx*Ly; 49 | if kx ~= 0 50 | hk = hk * 0.5; 51 | end 52 | if ky ~= 0 53 | hk = hk * 0.5; 54 | end 55 | hk = sqrt(hk); 56 | 57 | Bjx = Bjx + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-kx * pi/Lx) * sin(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly); 58 | Bjy = Bjy + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-ky * pi/Ly) * cos(kx * pi * xrel/Lx) * sin(ky * pi * yrel/Ly); 59 | end 60 | end 61 | 62 | Bjnorm = sqrt(Bjx*Bjx + Bjy*Bjy); 63 | 64 | %% OBSTACLE AVOIDANCE - Reference: Motion planning and collision avoidance using navigation vector fields 65 | 66 | Fox = 0; 67 | Foy = 0; 68 | product_sigma = 1; 69 | %account for other agents 70 | for other_agent=1:Nagents 71 | if other_agent ~= iagent 72 | ro = 0.01;%radius of obstacle 73 | re = 0.01;%safety margin 74 | r_robot= 0.00;% radius of robot 75 | 76 | % other agents positions 77 | xo = [posagents(other_agent,1)]; 78 | yo = [posagents(other_agent,2)]; 79 | 80 | %direction of vector fields 81 | p = ([Bjx,Bjy]./Bjnorm); 82 | 83 | % refer to the paper sec III 84 | smoothness = 0.05; 85 | r = ro + r_robot + re + smoothness; 86 | BF = ro^2 - r^2; 87 | BZ = ro^2 - (ro + r_robot +re)^2; 88 | 89 | dx = posagents(iagent, 1)-xo; 90 | dy = posagents(iagent, 2)-yo; 91 | 92 | B = ro^2 - dx^2 - dy^2; 93 | 94 | if(B <= BF || B > 0) 95 | sigma = 1; 96 | elseif (B > BF && B < BZ) 97 | % a = 2/(BZ- BF)^3; 98 | % b = -3*(BZ+BF)/(BZ-BF)^3; 99 | % c = 6*BZ*BF/(BZ-BF)^3; 100 | % d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 101 | % sigma =a*B^3 + b*B^2 + c*B + d; 102 | sigma =1-abs((B - BF)/(BF-BZ)); 103 | else 104 | sigma = 0; 105 | end 106 | product_sigma = product_sigma * sigma; 107 | if (p*[dx; dy] >= 0) 108 | Fox = Fox + (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 109 | Foy = Foy + (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 110 | end 111 | 112 | if (p*[dx; dy]<0) 113 | Fox = Fox + (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 114 | Foy = Foy +(1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 115 | end 116 | end 117 | end 118 | 119 | % account for obstacles 120 | for iobstacle = 1:obstacles.number 121 | 122 | ro = obstacles.r(iobstacle);%radius of obstacle 123 | re = 0.01;%safety margin 124 | r_robot= 0.00;% radius of robot 125 | 126 | % obstacles' positions 127 | xo = [obstacles.p(1,iobstacle)]; 128 | yo = [obstacles.p(2,iobstacle)]; 129 | 130 | %direction of vector fields 131 | p = ([Bjx,Bjy]./Bjnorm); 132 | 133 | % refer to the paper sec III 134 | smoothness = 0.05; 135 | r = ro + r_robot + re + smoothness; 136 | BF = ro^2 - r^2; 137 | BZ = ro^2 - (ro + r_robot +re)^2; 138 | 139 | dx = posagents(iagent, 1)-xo; 140 | dy = posagents(iagent, 2)-yo; 141 | 142 | B = ro^2 - dx^2 - dy^2; 143 | 144 | if(B <= BF || B > 0) 145 | sigma = 1; 146 | elseif (B > BF && B < BZ) 147 | % a = 2/(BZ- BF)^3; 148 | % b = -3*(BZ+BF)/(BZ-BF)^3; 149 | % c = 6*BZ*BF/(BZ-BF)^3; 150 | % d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 151 | % sigma =a*B^3 + b*B^2 + c*B + d; 152 | sigma =1-abs((B - BF)/(BF-BZ)); 153 | else 154 | sigma = 0; 155 | end 156 | product_sigma = product_sigma * sigma; 157 | if (p*[dx; dy] >= 0) 158 | Fox = Fox + (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 159 | Foy = Foy + (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 160 | end 161 | 162 | if (p*[dx; dy]<0) 163 | Fox = Fox + (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 164 | Foy = Foy +(1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 165 | end 166 | end 167 | Fox = Fox - product_sigma * Bjx/Bjnorm; 168 | Foy = Foy - product_sigma * Bjy/Bjnorm; 169 | L = sqrt(Fox^2 + Foy^2); 170 | 171 | % Updating agent location based on SMC feedback control law 172 | posagents(iagent,:) = posagents(iagent,:) + AgentSpeed * [Fox/L,Foy/L] * dt; 173 | 174 | % reflecting agent in case it goes out of domain bounds 175 | posagents(iagent, :) = reflect_agent(posagents(iagent, :),DomainBounds);%hadi no Need for this 176 | 177 | end 178 | 179 | end 180 | 181 | function [agent] = reflect_agent(agent, DomainBounds) 182 | 183 | xmin = DomainBounds.xmin; 184 | xmax = DomainBounds.xmax; 185 | ymin = DomainBounds.ymin; 186 | ymax = DomainBounds.ymax; 187 | 188 | if agent(1) < xmin 189 | agent(1) = xmin + (xmin - agent(1, 1)); 190 | end 191 | if agent(1) > xmax 192 | agent(1) = xmax - (agent(1, 1) - xmax); 193 | end 194 | if agent(2) < ymin 195 | agent(2) = ymin + (ymin - agent(1, 2)); 196 | end 197 | if agent(2) > ymax 198 | agent(2) = ymax - (agent(1, 2) - ymax); 199 | end 200 | 201 | end 202 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/coverage metric.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/coverage metric.fig -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/end.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/end.fig -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/main.m: -------------------------------------------------------------------------------- 1 | %% BY: Hadi Salman 2 | % 10/09/2016 3 | %% This codes prevents agent-agent collisions.(dynamic-obstacle avoidance) 4 | %%JUST CLICK RUN! MAKE SURE YOU DON'T START IN AN OBSTACLE REGION 5 | %% Setting domain bounds 6 | clear all; close all; 7 | DomainBounds.xmin = 0.0; 8 | DomainBounds.xmax = 1.0; 9 | DomainBounds.ymin = 0.0; 10 | DomainBounds.ymax = 1.0; 11 | Lx = DomainBounds.xmax - DomainBounds.xmin; 12 | Ly = DomainBounds.ymax - DomainBounds.ymin; 13 | 14 | %obstacles 15 | obstacles.r = [0.1,0.05,0.08,0.05];% radii of obstacles 16 | obstacles.p = [[0.5;0.5],[0.2;0.3],[0.7;0.2],[0.3;0.8]];%positions of obstacles[x1 y1 17 | % x2 y2 18 | % .....] 19 | obstacles.number = numel(obstacles.r); 20 | 21 | % Number of wave-numbers to be used 22 | Nk = 50;%% 23 | livePlot = true; %set if you want live plot for trajectories, other for faster execution 24 | %% Calculating muk 25 | res = 100;% resolution of discretization 26 | xdel=Lx/res; 27 | ydel=Ly/res; 28 | 29 | mu=ones(res,res); 30 | for xRange=0:xdel:Lx-xdel 31 | for yRange=0:ydel:Ly-ydel 32 | for i=1:obstacles.number 33 | if (xRange-obstacles.p(1,i))^2 + (yRange-obstacles.p(2,i))^2 <= obstacles.r(i)^2 34 | mu(uint8(xRange*res+1),uint8(yRange*res+1)) = 0; 35 | end 36 | end 37 | end 38 | end 39 | mu(50:90,70:90) = 0; 40 | mu=mu./sum(sum(mu)); 41 | [X,Y]=meshgrid(1:res,1:res); 42 | 43 | muk=zeros(Nk,Nk); 44 | Nkx = size(muk, 1); 45 | Nky = size(muk, 2); 46 | for kx = 0:Nkx-1 47 | for ky = 0:Nky-1 48 | 49 | hk=Lx*Ly; %using lim x->0 sinx/x=1 50 | if kx ~= 0 51 | hk = hk * 0.5; 52 | end 53 | if ky ~= 0 54 | hk = hk * 0.5; 55 | end 56 | hk = sqrt(hk); 57 | 58 | for xRange=0:xdel:Lx-xdel 59 | for yRange=0:ydel:Ly-ydel 60 | muk(kx+1, ky+1) = muk(kx+1, ky+1)+ mu(uint8(xRange*res+1),uint8(yRange*res+1)) *(1/hk)*cos(kx * pi * xRange/Lx) * cos(ky * pi * yRange/Ly); 61 | 62 | end 63 | end 64 | 65 | 66 | end 67 | end 68 | 69 | %% Initializing agent locations 70 | Nagents = 4; 71 | posagents = [0.35,0.7;0.05,0.3;0.1,0.95;0.83,0.03];%make sure not to start in an obstacle region! 72 | AgentSpeed = 5; 73 | 74 | colors = ['m','g','b','c']; 75 | 76 | Nsteps = 5000; 77 | dt = 0.001; 78 | 79 | % Initializing Fourier coefficients of coverage distribution 80 | Ck = zeros(Nk, Nk); 81 | 82 | figure(1); hold on; 83 | viscircles(obstacles.p',obstacles.r); 84 | for xRange=0:xdel:Lx-xdel 85 | for yRange=0:ydel:Ly-ydel 86 | for i=1:obstacles.number 87 | if (xRange-obstacles.p(1,i))^2 + (yRange-obstacles.p(2,i))^2 <= obstacles.r(i)^2 88 | scatter(xRange, yRange,2,'r','fill'); 89 | end 90 | end 91 | end 92 | end 93 | [X_avoid,Y_avoid] = meshgrid([.50:.01:.90],[.70:.01:.90]); 94 | scatter(X_avoid(:),Y_avoid(:),30,'filled','MarkerFaceColor',[.8,.8,.8]) 95 | axis equal; 96 | axis([0,1,0,1]) 97 | %% 98 | ck_t = zeros(Nk, Nk); 99 | phi_squared = zeros(Nsteps,1); 100 | s= 1.5; 101 | for iagent = 1:Nagents 102 | h_traj(iagent) = plot(posagents(iagent, 1), posagents(iagent, 2), 'Color', colors(iagent) , 'Marker', 'o', 'MarkerSize', 1); 103 | hold on 104 | h_scatter(iagent) = scatter(posagents(iagent, 1),posagents(iagent, 2),30,'filled','MarkerFaceColor',colors(iagent),'MarkerEdgeColor',[1,1,1]); 105 | axis([0 1 0 1]) 106 | axis equal 107 | end 108 | % Executing multiple steps of SMC algorithm 109 | Ergodicity_Metric_save=zeros(Nsteps,1); 110 | traj = zeros(Nsteps,Nagents,2); 111 | 112 | for it = 1:Nsteps 113 | time = it * dt; 114 | [posagents, Ck] = SMC_Update(posagents, Ck, muk, time, dt, DomainBounds, AgentSpeed,obstacles); 115 | traj(it,:,:) = posagents; 116 | ck_t = Ck/(Nagents*time); 117 | for iagent = 1:Nagents 118 | set(h_traj(iagent),'XData',traj(1:it,iagent,1),'YData',traj(1:it,iagent,2)) 119 | set(h_scatter(iagent),'XData',posagents(iagent, 1),'YData',posagents(iagent, 2)) 120 | if livePlot == true 121 | pause(0.001) 122 | end 123 | end 124 | if mod(it,100)==0 125 | fprintf('Iteration: %i/%i \n', it,Nsteps) 126 | end 127 | 128 | [Ergodicity_Metric] = Calculate_Ergodicity(Ck/Nagents/time, muk,DomainBounds); 129 | Ergodicity_Metric_save(it)=Ergodicity_Metric; 130 | 131 | end 132 | 133 | %% Plotting the metric of ergodicity 134 | time=0:0.001:0.001*(Nsteps); 135 | figure;loglog(time(2:end),Ergodicity_Metric_save(1:end)) 136 | axis([0.001 5 0.0001,1]) 137 | xlabel('Time'); 138 | ylabel('Coverage Metric, \phi(t)'); 139 | title('Metric of ergodicity as a function of time') 140 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/mid.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/mid.fig -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/nonuniform coverage.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/nonuniform coverage.mp4 -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/repulsive_vectorFields.m: -------------------------------------------------------------------------------- 1 | %%BY: Hadi Salman 2 | % OCT/05/2016 3 | %% Reference: Motion planning and collision avoidance using navigation vector fields 4 | % JUST CLICK RUN! 5 | 6 | %% 7 | 8 | ro = 1;%radius of obstacle 9 | re = 0.0;%safety margin 10 | r_robot= 0.0;% radius of robot 11 | 12 | figure; 13 | hold on 14 | viscircles([-2,-2],ro); 15 | axis equal; 16 | axis([-5 2 -5 2]); 17 | % scatter(0,0,'x'); 18 | % text(0,0,' target'); 19 | 20 | % obstacles' positions 21 | xo = [-2]; 22 | yo = [-2]; 23 | N = numel(xo); % number of obstacles 24 | 25 | % repulsive vector field (for one obstacle) 26 | phi = atan2(- yo, -xo)+ pi; 27 | p = [cos(phi), sin(phi)];% direction from obstacle to origin 28 | % p = [1,0]; 29 | resolution = 0.3; 30 | [x,y] = meshgrid(-5.0:resolution:2.0,-5.0:resolution:2.0); 31 | 32 | % %repalsive vector fields around obstacle o 33 | % %lambda = 1: p.deltaR > 0 (Check ref. paper sec: III) 34 | % Fox1 = p(2)*(x-xo).*(y-yo) - p(1)*(y-yo).^2; 35 | % Foy1 = p(1)*(x-xo).*(y-yo) - p(2)*(x-xo).^2; 36 | % 37 | % %lambda = 0 p.deltaR < 0 (Check ref. paper sec: III) 38 | % Fox0 = -p(1)*(x-xo).^2 - p(1)*(y-yo).^2; 39 | % Foy0 = -p(2)*(y-yo).^2 - p(2)*(x-xo).^2; 40 | 41 | %% 42 | % refer to the paper sec III 43 | %valur of B at some point r > rz = ro+re+r 44 | r = ro + r_robot + re + 1; 45 | BF = ro^2 - r^2; 46 | BZ = ro^2 - (ro + r_robot +re)^2; 47 | 48 | i=0; 49 | j=0; 50 | Fox = zeros(size(x)); 51 | Foy = zeros(size(x)); 52 | for xi = -5.0:resolution:2.0 53 | j=j+1; 54 | i=0; 55 | for yi = -5.0:resolution:2.0 56 | i=i+1; 57 | dx = xi-xo; 58 | dy = yi-yo; 59 | 60 | B = ro^2 - dx^2 - dy^2; 61 | 62 | if(B <= BF || B > 0) 63 | sigma = 1; 64 | elseif (B > BF && B < BZ) 65 | a = 2/(BZ- BF)^3; 66 | b = -3*(BZ+BF)/(BZ-BF)^3; 67 | c = 6*BZ*BF/(BZ-BF)^3; 68 | d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 69 | sigma =a*B^3 + b*B^2 + c*B + d; 70 | else 71 | sigma = 0; 72 | end 73 | 74 | if (p*[dx; dy] >= 0) 75 | Fox(i, j) = (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([(p(2)*(dx)*(dy) - p(1)*(dy)^2),(p(1)*(dx)*(dy) - p(2)*(dx)^2)]); 76 | Foy(i, j) = (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([(p(2)*(dx)*(dy) - p(1)*(dy)^2),(p(1)*(dx)*(dy) - p(2)*(dx)^2)]); 77 | end 78 | 79 | if (p*[dx; dy]<0) 80 | Fox(i, j) = (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([(-p(1)*(dx).^2 - p(1)*(dy).^2),(-p(2)*(dy).^2 - p(2)*(dx).^2)]); 81 | Foy(i, j) = (1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([(-p(1)*(dx).^2 - p(1)*(dy).^2),(-p(2)*(dy).^2 - p(2)*(dx).^2)]); 82 | end 83 | end 84 | end 85 | 86 | % Fox = Fox1; 87 | % Foy = Foy1; 88 | % figure(1) 89 | L = sqrt(Fox.^2 + Foy.^2); 90 | quiver(x,y,Fox./L,Foy./L); 91 | 92 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/start.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/fig_9_nonuniformCoverage/start.fig -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/section4c_dynamicsObstacles/Calculate_Ergodicity.m: -------------------------------------------------------------------------------- 1 | function [Ergodicity_Metric] = Calculate_Ergodicity(Ck, muk, DomainBounds) 2 | Lx = DomainBounds.xmax - DomainBounds.xmin; 3 | Ly = DomainBounds.ymax - DomainBounds.ymin; 4 | Nkx = size(muk, 1); 5 | Nky = size(muk, 2); 6 | Nagents = 1; 7 | 8 | Ergodicity_Metric=0; 9 | for iagent = 1:Nagents 10 | for kx = 0:Nkx-1 11 | for ky = 0:Nky-1 12 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^1.5); 13 | Ergodicity_Metric=Ergodicity_Metric+lambda_k*(abs(Ck(kx+1, ky+1)-muk(kx+1, ky+1)))^2; 14 | end 15 | end 16 | 17 | end 18 | % display('Ergodicity_Metric:') 19 | end 20 | 21 | 22 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/section4c_dynamicsObstacles/SMC_Update.m: -------------------------------------------------------------------------------- 1 | %%BY: Hadi Salman 2 | % OCT/09/2016 3 | function [posagents, Ck] = SMC_Update(posagents, Ck, muk, time, dt, DomainBounds, AgentSpeed, obstacles) 4 | 5 | Lx = DomainBounds.xmax - DomainBounds.xmin; 6 | Ly = DomainBounds.ymax - DomainBounds.ymin; 7 | xmin = DomainBounds.xmin; 8 | ymin = DomainBounds.ymin; 9 | 10 | Nkx = size(muk, 1); 11 | Nky = size(muk, 2); 12 | Nagents = size(posagents, 1); 13 | 14 | % Updating Fourier Coefficients of Coverage Distribution 15 | for iagent=1:Nagents 16 | xrel = posagents(iagent, 1) - xmin; 17 | yrel = posagents(iagent, 2) - ymin; 18 | for kx = 0:Nkx-1 19 | for ky = 0:Nky-1 20 | 21 | hk = Lx*Ly; 22 | if kx ~= 0 23 | hk = hk * 0.5; 24 | end 25 | if ky ~= 0 26 | hk = hk * 0.5; 27 | end 28 | hk = sqrt(hk); 29 | 30 | Ck(kx+1, ky+1) = Ck(kx+1, ky+1) + (1/hk)*cos(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly) * dt; 31 | end 32 | end 33 | end 34 | 35 | % Computing controls 36 | s=1.5; 37 | 38 | for iagent = 1:Nagents 39 | Bjx = 0.0; 40 | Bjy = 0.0; 41 | 42 | xrel = posagents(iagent, 1) - xmin; 43 | yrel = posagents(iagent, 2) - ymin; 44 | for kx = 0:Nkx-1 45 | for ky = 0:Nky-1 46 | lambda_k = 1.0 / ((1.0 + kx * kx + ky * ky)^s); 47 | 48 | hk = Lx*Ly; 49 | if kx ~= 0 50 | hk = hk * 0.5; 51 | end 52 | if ky ~= 0 53 | hk = hk * 0.5; 54 | end 55 | hk = sqrt(hk); 56 | 57 | Bjx = Bjx + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-kx * pi/Lx) * sin(kx * pi * xrel/Lx) * cos(ky * pi * yrel/Ly); 58 | Bjy = Bjy + (lambda_k / hk) * (Ck(kx+1, ky+1) - Nagents*time*muk(kx+1, ky+1)) * (-ky * pi/Ly) * cos(kx * pi * xrel/Lx) * sin(ky * pi * yrel/Ly); 59 | end 60 | end 61 | 62 | Bjnorm = sqrt(Bjx*Bjx + Bjy*Bjy); 63 | 64 | %% OBSTACLE AVOIDANCE - Reference: Motion planning and collision avoidance using navigation vector fields 65 | 66 | Fox = 0; 67 | Foy = 0; 68 | product_sigma = 1; 69 | %account for other agents 70 | for other_agent=1:Nagents 71 | if other_agent ~= iagent 72 | ro = 0.01;%radius of obstacle 73 | re = 0.01;%safety margin 74 | r_robot= 0.00;% radius of robot 75 | 76 | % other agents positions 77 | xo = [posagents(other_agent,1)]; 78 | yo = [posagents(other_agent,2)]; 79 | 80 | %direction of vector fields 81 | p = ([Bjx,Bjy]./Bjnorm); 82 | 83 | % refer to the paper sec III 84 | smoothness = 0.01; 85 | r = ro + r_robot + re + smoothness; 86 | BF = ro^2 - r^2; 87 | BZ = ro^2 - (ro + r_robot +re)^2; 88 | 89 | dx = posagents(iagent, 1)-xo; 90 | dy = posagents(iagent, 2)-yo; 91 | 92 | B = ro^2 - dx^2 - dy^2; 93 | 94 | if(B <= BF || B > 0) 95 | sigma = 1; 96 | elseif (B > BF && B < BZ) 97 | % a = 2/(BZ- BF)^3; 98 | % b = -3*(BZ+BF)/(BZ-BF)^3; 99 | % c = 6*BZ*BF/(BZ-BF)^3; 100 | % d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 101 | % sigma =a*B^3 + b*B^2 + c*B + d; 102 | sigma =1-abs((B - BF)/(BF-BZ)); 103 | else 104 | sigma = 0; 105 | end 106 | product_sigma = product_sigma * sigma; 107 | if (p*[dx; dy] >= 0) 108 | Fox = Fox + (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 109 | Foy = Foy + (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 110 | end 111 | 112 | if (p*[dx; dy]<0) 113 | Fox = Fox + (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 114 | Foy = Foy +(1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 115 | end 116 | end 117 | end 118 | 119 | % account for obstacles 120 | for iobstacle = 1:obstacles.number 121 | 122 | ro = obstacles.r(iobstacle);%radius of obstacle 123 | re = 0.05;%safety margin 124 | r_robot= 0.00;% radius of robot 125 | 126 | % obstacles' positions 127 | xo = [obstacles.p(1,iobstacle)]; 128 | yo = [obstacles.p(2,iobstacle)]; 129 | 130 | %direction of vector fields 131 | p = ([Bjx,Bjy]./Bjnorm); 132 | 133 | % refer to the paper sec III 134 | smoothness = 0.1; 135 | r = ro + r_robot + re + smoothness; 136 | BF = ro^2 - r^2; 137 | BZ = ro^2 - (ro + r_robot +re)^2; 138 | 139 | dx = posagents(iagent, 1)-xo; 140 | dy = posagents(iagent, 2)-yo; 141 | 142 | B = ro^2 - dx^2 - dy^2; 143 | 144 | if(B <= BF || B > 0) 145 | sigma = 1; 146 | elseif (B > BF && B < BZ) 147 | % a = 2/(BZ- BF)^3; 148 | % b = -3*(BZ+BF)/(BZ-BF)^3; 149 | % c = 6*BZ*BF/(BZ-BF)^3; 150 | % d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 151 | % sigma =a*B^3 + b*B^2 + c*B + d; 152 | sigma =1-abs((B - BF)/(BF-BZ)); 153 | else 154 | sigma = 0; 155 | end 156 | product_sigma = product_sigma * sigma; 157 | if (p*[dx; dy] >= 0) 158 | Fox = Fox + (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 159 | Foy = Foy + (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([p(2)*(dx)*(dy) - p(1)*(dy)^2,p(1)*(dx)*(dy) - p(2)*(dx)^2]); 160 | end 161 | 162 | if (p*[dx; dy]<0) 163 | Fox = Fox + (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 164 | Foy = Foy +(1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([-p(1)*(dx).^2 - p(1)*(dy).^2,-p(2)*(dy).^2 - p(2)*(dx).^2]); 165 | end 166 | end 167 | Fox = Fox - product_sigma * Bjx/Bjnorm; 168 | Foy = Foy - product_sigma * Bjy/Bjnorm; 169 | L = sqrt(Fox^2 + Foy^2); 170 | 171 | % Updating agent location based on SMC feedback control law 172 | posagents(iagent,:) = posagents(iagent,:) + AgentSpeed * [Fox/L,Foy/L] * dt; 173 | 174 | % reflecting agent in case it goes out of domain bounds 175 | posagents(iagent, :) = reflect_agent(posagents(iagent, :),DomainBounds);%hadi no Need for this 176 | 177 | end 178 | 179 | end 180 | 181 | function [agent] = reflect_agent(agent, DomainBounds) 182 | 183 | xmin = DomainBounds.xmin; 184 | xmax = DomainBounds.xmax; 185 | ymin = DomainBounds.ymin; 186 | ymax = DomainBounds.ymax; 187 | 188 | if agent(1) < xmin 189 | agent(1) = xmin + (xmin - agent(1, 1)); 190 | end 191 | if agent(1) > xmax 192 | agent(1) = xmax - (agent(1, 1) - xmax); 193 | end 194 | if agent(2) < ymin 195 | agent(2) = ymin + (ymin - agent(1, 2)); 196 | end 197 | if agent(2) > ymax 198 | agent(2) = ymax - (agent(1, 2) - ymax); 199 | end 200 | 201 | end 202 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/section4c_dynamicsObstacles/main.m: -------------------------------------------------------------------------------- 1 | %% BY: Hadi Salman 2 | % 10/09/2016 3 | %%This codes prevents agent-agent collisions.(dynamic-obstacle avoidance) 4 | %%JUST CLICK RUN! MAKE SURE YOU DON'T START IN AN OBSTACLE REGION 5 | %% Setting domain bounds 6 | DomainBounds.xmin = 0.0; 7 | DomainBounds.xmax = 1.0; 8 | DomainBounds.ymin = 0.0; 9 | DomainBounds.ymax = 1.0; 10 | Lx = DomainBounds.xmax - DomainBounds.xmin; 11 | Ly = DomainBounds.ymax - DomainBounds.ymin; 12 | 13 | % Number of wave-numbers to be used 14 | Nk = 50;%% 15 | 16 | %% Calculating muk 17 | res = 100;% resolution of discretization 18 | xdel=Lx/res; 19 | ydel=Ly/res; 20 | 21 | mu=ones(res,res); 22 | 23 | mu=mu./sum(sum(mu)); 24 | [X,Y]=meshgrid(1:res,1:res); 25 | 26 | muk=zeros(Nk,Nk); 27 | Nkx = size(muk, 1); 28 | Nky = size(muk, 2); 29 | for kx = 0:Nkx-1 30 | for ky = 0:Nky-1 31 | 32 | hk=Lx*Ly; %using lim x->0 sinx/x=1 33 | if kx ~= 0 34 | hk = hk * 0.5; 35 | end 36 | if ky ~= 0 37 | hk = hk * 0.5; 38 | end 39 | hk = sqrt(hk); 40 | 41 | for xRange=0:xdel:Lx-xdel 42 | for yRange=0:ydel:Ly-ydel 43 | muk(kx+1, ky+1) = muk(kx+1, ky+1)+ mu(uint8(xRange*res+1),uint8(yRange*res+1)) *(1/hk)*cos(kx * pi * xRange/Lx) * cos(ky * pi * yRange/Ly); 44 | 45 | end 46 | end 47 | 48 | 49 | end 50 | end 51 | 52 | %% Initializing agent locations 53 | Nagents = 4; 54 | posagents = [0.35,0.7;0.05,0.3;0.1,0.95;0.83,0.03];%make sure not to start in an obstacle region! 55 | AgentSpeed = 5; 56 | 57 | colors = ['m','g','b','c']; 58 | 59 | Nsteps = 5000; 60 | dt = 0.001; 61 | 62 | % Initializing Fourier coefficients of coverage distribution 63 | Ck = zeros(Nk, Nk); 64 | 65 | %obstacles 66 | 67 | obstacles.r = [0.1,0.05,0.05];% radii of obstacles 68 | obstacles_pos = @(it)[[0.1*cos(2*pi*it/3000)+0.5;0.2*sin(2*pi*it/1000)+0.5], ... 69 | [0.2;0.2*sin(2*pi*it/2000)+0.6],.... 70 | [0.2*sin(2*pi*it/1000)+0.3;0.2]];%,[0.2;0.3],[0.7;0.2],[0.3;0.8]positions of obstacles[x1 y1 71 | % x2 y2 72 | % .....] 73 | obstacles.number = numel(obstacles.r); 74 | 75 | figure(1);hold on 76 | 77 | ck_t = zeros(Nk, Nk); 78 | phi_squared = zeros(Nsteps,1); 79 | s= 1.5; 80 | 81 | 82 | % Executing multiple steps of SMC algorithm 83 | Ergodicity_Metric_save=0; 84 | for it = 1:Nsteps 85 | time = it * dt; 86 | obstacles.p = obstacles_pos(it); 87 | [posagents, Ck] = SMC_Update(posagents, Ck, muk, time, dt, DomainBounds, AgentSpeed,obstacles); 88 | ck_t = Ck/(Nagents*time); 89 | for iagent = 1:Nagents 90 | % plot(posagents(iagent, 1), posagents(iagent, 2), 'Color', colors(iagent) , 'Marker', 'o', 'MarkerSize', 1); 91 | axis([0 1 0 1]) 92 | axis equal 93 | xlim([0,1]) 94 | ylim([0,1]) 95 | if it ~= 1 96 | h(iagent).Visible = 'off'; 97 | end 98 | h(iagent) = scatter(posagents(iagent, 1), posagents(iagent, 2),colors(iagent),'fill'); 99 | viscircles(obstacles_pos(it)',obstacles.r); 100 | pause(0.001) 101 | end 102 | 103 | if mod(it,100)==0 104 | fprintf('Iteration: %i/%i \n', it,Nsteps) 105 | end 106 | [Ergodicity_Metric] = Calculate_Ergodicity(Ck/Nagents/time, muk,DomainBounds); 107 | Ergodicity_Metric_save=[Ergodicity_Metric_save,Ergodicity_Metric]; 108 | 109 | end 110 | 111 | %% Plotting the metric of ergodicity 112 | close all 113 | time=0:0.001:0.001*(Nsteps); 114 | figure;loglog(time(2:end),Ergodicity_Metric_save(2:end)) 115 | axis([0.001 5 0.0001,1]) 116 | xlabel('Time'); 117 | ylabel('Coverage Metric, \phi(t)'); 118 | -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/section4c_dynamicsObstacles/metric.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Hadisalman/ICAPS2017-code/1032c1bad29962b9a9c52bf772256649ad0502ab/Codes For ICAPS2017 paper/section4c_dynamicsObstacles/metric.png -------------------------------------------------------------------------------- /Codes For ICAPS2017 paper/section4c_dynamicsObstacles/repulsive_vectorFields.m: -------------------------------------------------------------------------------- 1 | %%BY: Hadi Salman 2 | % NOV/18/2016 3 | %% Reference: Motion planning and collision avoidance using navigation vector fields 4 | % JUST CLICK RUN! 5 | 6 | %% 7 | ro = 1;%radius of obstacle 8 | re = 0.0;%safety margin 9 | r_robot= 0.0;% radius of robot 10 | 11 | figure; 12 | hold on 13 | viscircles([-2,-2],ro); 14 | axis equal; 15 | axis([-5 2 -5 2]); 16 | % scatter(0,0,'x'); 17 | % text(0,0,' target'); 18 | 19 | % obstacles' positions 20 | xo = [-2]; 21 | yo = [-2]; 22 | N = numel(xo); % number of obstacles 23 | 24 | % repulsive vector field (for one obstacle) 25 | phi = atan2(- yo, -xo)+ pi; 26 | p = [cos(phi), sin(phi)];% direction from obstacle to origin 27 | % p = [1,0]; 28 | resolution = 0.3; 29 | [x,y] = meshgrid(-5.0:resolution:2.0,-5.0:resolution:2.0); 30 | 31 | % %repalsive vector fields around obstacle o 32 | % %lambda = 1: p.deltaR > 0 (Check ref. paper sec: III) 33 | % Fox1 = p(2)*(x-xo).*(y-yo) - p(1)*(y-yo).^2; 34 | % Foy1 = p(1)*(x-xo).*(y-yo) - p(2)*(x-xo).^2; 35 | % 36 | % %lambda = 0 p.deltaR < 0 (Check ref. paper sec: III) 37 | % Fox0 = -p(1)*(x-xo).^2 - p(1)*(y-yo).^2; 38 | % Foy0 = -p(2)*(y-yo).^2 - p(2)*(x-xo).^2; 39 | 40 | %% 41 | % refer to the paper sec III 42 | %valur of B at some point r > rz = ro+re+r 43 | r = ro + r_robot + re + 1; 44 | BF = ro^2 - r^2; 45 | BZ = ro^2 - (ro + r_robot +re)^2; 46 | 47 | i=0; 48 | j=0; 49 | Fox = zeros(size(x)); 50 | Foy = zeros(size(x)); 51 | for xi = -5.0:resolution:2.0 52 | j=j+1; 53 | i=0; 54 | for yi = -5.0:resolution:2.0 55 | i=i+1; 56 | dx = xi-xo; 57 | dy = yi-yo; 58 | 59 | B = ro^2 - dx^2 - dy^2; 60 | 61 | if(B <= BF || B > 0) 62 | sigma = 1; 63 | elseif (B > BF && B < BZ) 64 | a = 2/(BZ- BF)^3; 65 | b = -3*(BZ+BF)/(BZ-BF)^3; 66 | c = 6*BZ*BF/(BZ-BF)^3; 67 | d = BZ^2*(BZ - 3*BF)/(BZ-BF)^3; 68 | sigma =a*B^3 + b*B^2 + c*B + d; 69 | else 70 | sigma = 0; 71 | end 72 | 73 | if (p*[dx; dy] >= 0) 74 | Fox(i, j) = (1-sigma)*(p(2)*(dx)*(dy) - p(1)*(dy)^2)/norm([(p(2)*(dx)*(dy) - p(1)*(dy)^2),(p(1)*(dx)*(dy) - p(2)*(dx)^2)]); 75 | Foy(i, j) = (1-sigma)*(p(1)*(dx)*(dy) - p(2)*(dx)^2)/norm([(p(2)*(dx)*(dy) - p(1)*(dy)^2),(p(1)*(dx)*(dy) - p(2)*(dx)^2)]); 76 | end 77 | 78 | if (p*[dx; dy]<0) 79 | Fox(i, j) = (1-sigma)*(-p(1)*(dx).^2 - p(1)*(dy).^2)/norm([(-p(1)*(dx).^2 - p(1)*(dy).^2),(-p(2)*(dy).^2 - p(2)*(dx).^2)]); 80 | Foy(i, j) = (1-sigma)*(-p(2)*(dy).^2 - p(2)*(dx).^2)/norm([(-p(1)*(dx).^2 - p(1)*(dy).^2),(-p(2)*(dy).^2 - p(2)*(dx).^2)]); 81 | end 82 | end 83 | end 84 | 85 | % Fox = Fox1; 86 | % Foy = Foy1; 87 | % figure(1) 88 | L = sqrt(Fox.^2 + Foy.^2); 89 | quiver(x,y,Fox./L,Foy./L); 90 | 91 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Hadisalman 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ICAPS2017-code 2 | Repository containing the codes that were used in the [Multi-agent Ergodic Coverage with Obstacle Avoidance](http://biorobotics.ri.cmu.edu/papers/paperUploads/15731-68931-1-PB.pdf) paper. 3 | 4 | # How to run 5 | Each folder in the repository correpsonds to a figure in the paper. To generate the results of each folder, simply run the `main.m` file in that folder. 6 | 7 | # Contact 8 | If you find any bug or have any question, please contact me! 9 | * Hadi Salman `hadicsalman at gmail.com` 10 | --------------------------------------------------------------------------------