added IIP and HS folders

Matlab Code/checkCollisions.m → HS/checkCollisions.m

@@ -9,7 +9,11 @@ collision_flag = false;
 for i=1:step_num
    
     % look for collisions (there is a faster way to do this, i am sure)
-    dists = sqrt((pt(1) - obs_points(:,1)).^2 + (pt(2) - obs_points(:,2)).^2);
+    if ~isempty(obs_points)
+        dists = sqrt((pt(1) - obs_points(:,1)).^2 + (pt(2) - obs_points(:,2)).^2);
+    else 
+        dists = [];
+    end
     if any(dists <= dx)
         % collision!
         collision_flag = true;

HS/checkConnected.m

+function connected_flag = checkConnected(G,verts)
+
+if nargin == 1
+    [~,C] = conncompAdj(G.adj);
+elseif nargin == 2
+    % trim to only examine induced graph of verts
+    rows = G.adj(verts,:);
+    ind_adj = rows(:,verts);
+    [~,C] = conncompAdj(ind_adj);    
+end
+
+if all(C == 1)
+    connected_flag = true;
+else
+    connected_flag = false;
+end
+
+end
\ No newline at end of file

HS/conncompAdj.m

+function [S,C] = conncompAdj(G)
+[p,q,r] = dmperm(G'+speye(size(G)));
+S = numel(r)-1;
+C = cumsum(full(sparse(1,r(1:end-1),1,1,size(G,1))));
+C(p) = C;
+% C = C-1; % return comps - 1
+S = S - 1;
+end
\ No newline at end of file

HS/dijkstra.m

+function [dist,path] = dijkstra(G,start_id,d_max)
+%DIJKSTRA Calculates the shortest distance and path between points on a map
+%   using Dijkstra's Shortest Path Algorithm
+% 
+% [DIST, PATH] = DIJKSTRA(NODES, SEGMENTS, SID, FID)
+%   Calculates the shortest distance and path between start and finish nodes SID and FID
+% 
+% [DIST, PATH] = DIJKSTRA(NODES, SEGMENTS, SID)
+%   Calculates the shortest distances and paths from the starting node SID to all
+%     other nodes in the map
+% 
+% Note:
+%     DIJKSTRA is set up so that an example is created if no inputs are provided,
+%       but ignores the example and just processes the inputs if they are given.
+% 
+% Inputs:
+%     NODES should be an Nx3 or Nx4 matrix with the format [ID X Y] or [ID X Y Z]
+%       where ID is an integer, and X, Y, Z are cartesian position coordinates)
+%     SEGMENTS should be an Mx3 matrix with the format [ID N1 N2]
+%       where ID is an integer, and N1, N2 correspond to node IDs from NODES list
+%       such that there is an [undirected] edge/segment between node N1 and node N2
+%     SID should be an integer in the node ID list corresponding with the starting node
+%     FID (optional) should be an integer in the node ID list corresponding with the finish
+% 
+% Outputs:
+%     DIST is the shortest Euclidean distance
+%       If FID was specified, DIST will be a 1x1 double representing the shortest
+%         Euclidean distance between SID and FID along the map segments. DIST will have
+%         a value of INF if there are no segments connecting SID and FID.
+%       If FID was not specified, DIST will be a 1xN vector representing the shortest
+%         Euclidean distance between SID and all other nodes on the map. DIST will have
+%         a value of INF for any nodes that cannot be reached along segments of the map.
+%     PATH is a list of nodes containing the shortest route
+%       If FID was specified, PATH will be a 1xP vector of node IDs from SID to FID.
+%         NAN will be returned if there are no segments connecting SID to FID.
+%       If FID was not specified, PATH will be a 1xN cell of vectors representing the
+%         shortest route from SID to all other nodes on the map. PATH will have a value
+%         of NAN for any nodes that cannot be reached along the segments of the map.
+% 
+% Example:
+%     dijkstra; % calculates shortest path and distance between two nodes
+%               % on a map of randomly generated nodes and segments
+% 
+% Example:
+%     nodes = [(1:10); 100*rand(2,10)]';
+%     segments = [(1:17); floor(1:0.5:9); ceil(2:0.5:10)]';
+%     figure; plot(nodes(:,2), nodes(:,3),'k.');
+%     hold on;
+%     for s = 1:17
+%         if (s <= 10) text(nodes(s,2),nodes(s,3),[' ' num2str(s)]); end
+%         plot(nodes(segments(s,2:3)',2),nodes(segments(s,2:3)',3),'k');
+%     end
+%     [d, p] = dijkstra(nodes, segments, 1, 10)
+%     for n = 2:length(p)
+%         plot(nodes(p(n-1:n),2),nodes(p(n-1:n),3),'r-.','linewidth',2);
+%     end
+%     hold off;
+% 
+% Author: Joseph Kirk
+% Email: jdkirk630 at gmail dot com
+% Release: 1.3
+% Release Date: 5/18/07
+if (nargin < 2) % SETUP
+    error("provide more inputs");
+    
+else %--------------------------------------------------------------------------
+    % MAIN FUNCTION - DIJKSTRA'S ALGORITHM
+    
+    % initializations
+    node_ids = (1:G.N)';
+    [a,b] = find(G.adj);
+    segments = [(1:length(a))', a, b];
+    nodes = [G.nodes(:,3), G.nodes(:,1:2)];
+    [num_map_pts,cols] = size(nodes);
+    table = zeros(num_map_pts,2);
+    shortest_distance = Inf(num_map_pts,1);
+    settled = zeros(num_map_pts,1);
+    path = num2cell(NaN(num_map_pts,1));
+    col = 2;
+    pidx = find(start_id == node_ids);
+    shortest_distance(pidx) = 0;
+    table(pidx,col) = 0;
+    settled(pidx) = 1;
+    path(pidx) = {start_id};
+    while sum(~settled) > 0
+        % update the table
+        table(:,col-1) = table(:,col);
+        table(pidx,col) = 0;
+        % find neighboring nodes in the segments list
+        neighbor_ids = [segments(node_ids(pidx) == segments(:,2),3);
+            segments(node_ids(pidx) == segments(:,3),2)];
+        % calculate the distances to the neighboring nodes and keep track of the paths
+        for k = 1:length(neighbor_ids)
+            cidx = find(neighbor_ids(k) == node_ids);
+            if ~settled(cidx)
+                d = sqrt(sum((nodes(pidx,2:cols) - nodes(cidx,2:cols)).^2));
+                if ((table(cidx,col-1) == 0) || ...
+                        (table(cidx,col-1) > (table(pidx,col-1) + d))) && ((table(pidx,col-1) + d) <= d_max) % key addition
+                    table(cidx,col) = table(pidx,col-1) + d;
+                    tmp_path = path(pidx);
+                    path(cidx) = {[tmp_path{1} neighbor_ids(k)]};
+                else
+                    table(cidx,col) = table(cidx,col-1);
+                end
+            end
+        end
+        % find the minimum non-zero value in the table and save it
+        nidx = find(table(:,col));
+        ndx = find(table(nidx,col) == min(table(nidx,col)));
+        if isempty(ndx)
+            break
+        else
+            pidx = nidx(ndx(1));
+            shortest_distance(pidx) = table(pidx,col);
+            settled(pidx) = 1;
+        end
+    end
+    % return the distance and path arrays for all of the nodes
+    dist = shortest_distance';
+    path = path';
+end
\ No newline at end of file

HS/generateEnvGraph.m

+function [G,Gc] = generateEnvGraph(environment_size, rho, move_size, obstacle_points, env_step_size)
+
+xbdd = environment_size(1:2);
+ybdd = environment_size(3:4);
+
+dx = 0.1 ; % fix this as a parameter later
+
+[X,Y] = meshgrid(xbdd(1):env_step_size:xbdd(2), ybdd(1):env_step_size:ybdd(2));
+G_points = [reshape(X,[],1), reshape(Y,[],1)];
+
+% check for obstacle collisions
+rm_index = zeros(size(G_points,1),1);
+for i=1:size(G_points,1)
+    if ~isempty(obstacle_points)
+        dists = sqrt((obstacle_points(:,1) - G_points(i,1)).^2 + (obstacle_points(:,2) - G_points(i,2)).^2);
+    else
+        dists = [];
+    end
+    if any(dists <= dx)
+        rm_index(i) = i; % delete this entry because it's too close to obstacles
+    end
+end
+G_points(rm_index(rm_index ~= 0),:) = []; 
+env_nodes = size(G_points, 1);
+
+% build adjacency matrix for G (this is the MOVEMENT adjacency)
+adjacency = zeros(env_nodes);
+for i=1:env_nodes
+    dists = sqrt((G_points(:,1) - G_points(i,1)).^2 + (G_points(:,2) - G_points(i,2)).^2);
+    if any(dists <= move_size)
+        close_nodes = find(dists <= move_size);
+        for j=1:size(close_nodes,1)
+            if close_nodes(j) == i
+                continue;
+            end
+            [collision_flag, ~] = checkCollisions(G_points(i,:), G_points(close_nodes(j),:), dx, obstacle_points);
+            if ~collision_flag
+                adjacency(i,close_nodes(j)) = 1;
+            end
+        end
+    end
+end
+
+% build adjacency matrix for G (this is the COMMUNICATION adjacency)
+adjacency_c = zeros(env_nodes);
+for i=1:env_nodes
+    dists = sqrt((G_points(:,1) - G_points(i,1)).^2 + (G_points(:,2) - G_points(i,2)).^2);
+    if any(dists <= rho)
+        close_nodes = find(dists <= rho);
+        for j=1:size(close_nodes,1)
+            if close_nodes(j) == i
+                continue;
+            end
+            [collision_flag, ~] = checkCollisions(G_points(i,:), G_points(close_nodes(j),:), dx, obstacle_points);
+            if ~collision_flag
+                adjacency_c(i,close_nodes(j)) = 1;
+            end
+        end
+    end
+end
+
+G = struct('N', size(G_points,1), 'nodes', [G_points, (1:size(G_points,1))'], 'adj', adjacency);
+Gc = struct('N', size(G_points,1), 'nodes', [G_points, (1:size(G_points,1))'], 'adj', adjacency_c);
+
+end
+

HS/generateObstacles.m

+function [x_pts, y_pts] = generateObstacles(env, threshold, dx, init_conf, final_conf, safe_radius)
+
+xdim = env(2) - env(1);
+ydim = env(4) - env(3);
+
+im = zeros(floor(xdim/dx), floor(ydim/dx)); 
+im = perlin_noise(im);
+obs_points = im;
+
+obs_points = obs_points + abs(min(min(obs_points))); % make it positive
+obs_points = obs_points/max(max(obs_points)); % make it normal
+obs_points = smoothdata(obs_points,1,'movmean',10);
+obs_points = smoothdata(obs_points,2,'movmean',10);
+
+[x_pts, y_pts] = find(obs_points <= threshold);
+x_pts = x_pts.*dx;
+y_pts = y_pts.*dx;
+
+% iterate through to get rid of points interferring with starting and
+% ending points
+% but do it for all points in initial and final configurations
+
+for i=1:size(init_conf,1)
+    init_d = sqrt((init_conf(i,1) - x_pts).^2 + (init_conf(i,2) - y_pts).^2);
+    x_pts = x_pts(init_d >= safe_radius);
+    y_pts = y_pts(init_d >= safe_radius);
+end
+for i=1:size(final_conf,1)
+    final_d = sqrt((final_conf(i,1) - x_pts).^2 + (final_conf(i,2) - y_pts).^2);
+    x_pts = x_pts(final_d >= safe_radius);
+    y_pts = y_pts(final_d >= safe_radius);
+end
+
+end
\ No newline at end of file

HS/getVerts.m

+function [vertices, order] = getVerts(V,robots,option)
+%simply a vertex extractor to get the vertices at the end of cell-table V for
+%each agent
+
+if nargin < 3
+    error("not enough inputs")
+end
+
+if strcmp(option, 'end')
+    vertices = zeros(length(robots),1);
+    for i=1:length(robots)
+        % pick out last non-zero element of V
+        r = robots(i);
+        vertices(i) = V{r}.node_list(find(V{r}.node_list,1,'last'));
+    end
+elseif strcmp(option, 'all')
+    vertices = zeros(length(robots)*length(V{1}.node_list),1);
+    order = vertices;
+    rowi = 1;
+    for i=1:length(robots)
+        r = robots(i);
+        tmp = nonzeros(V{r}.node_list);
+        vertices(rowi:(rowi+length(tmp)-1)) = tmp;
+        order(rowi:(rowi+length(tmp)-1)) = i;
+        rowi = rowi+length(tmp);
+    end
+    vertices = nonzeros(vertices);
+    order = nonzeros(order);
+end
+
+end
\ No newline at end of file

HS/main.m

+function [utility, computation_time, path_length] = main(input_env_step_size, m, v_level, input_N)
+
+% Nomenclature:
+% 	 K: number of robots
+% 	 G(N,E): environment graph, nodes are convex regions and edges are connections between regions
+% 	 G'(N',E'): time-unfolded version of G(N,E), a node in N' is a time-stamped copy of a node in N.
+%   That is, n' in N' corresponds to (n,t) for node n in N and time t.
+% 	 T: total discrete time steps (the planning horizon)
+% 	 k: robot k in {1,,K}
+% 	 P_k = {r_k(0),,r_k(T)}: path planned by robot k, where each r_k(t) \in N'
+% 	 C(P_k): the cost of path P_k, assume all moves have cost one, so C(P_k) = T for horizon T
+% 	 P = P_1 U P_2 U  U P_K:
+%   union of paths for all robots, represents which nodes are visited at which time
+% 	 F(P): team objective function, assume that F is known
+% 	 G_c(N, E_c): graph describing connectivity between nodes in G (node i in N and j in N are connected if (i,j) in E_c)
+% 	 P(t): a configuration of robots at time t using path P
+% 	 T_I: the disconnected interval, during which the network may choose to lose connectivity
+% 	 tau: the smallest integer such that tau*T_I > T
+
+addpath('C:\Users\Larkin\Google Drive\DropBox\VT\Research\SAR\Interaction Planning\MATLAB\RA-L_2020');
+
+N = input_N;
+load_params;
+
+env_step_size = input_env_step_size;
+% obstacle stuff
+[x_pts, y_pts] = generateObstacles(environment_size, threshold, obs_step_size, starting_locations, goal_locations, safe_radius);
+obstacle_points = [x_pts, y_pts];
+
+
+dummy = rrtAgent(N, mean(starting_locations,1), mean(goal_locations,1), token_ownership, ...
+    environment_size, obstacle_points, distance_increment, step_size, ...
+    planning_iterations, move_speed, collision_radius);
+
+t_start = tic;
+
+dummy = dummy.planMultiGoalPath;
+if isempty(dummy.path_waypoints)
+    utility = -1;
+    tmp = toc(t_start);
+    computation_time = -1;
+    path_length = -1;
+    return
+end
+
+[G, Gc] = generateEnvGraph(environment_size, rho, move_size, obstacle_points, env_step_size);
+
+starting_verts = zeros(N,1);
+for i=1:N
+    dists = (G.nodes(:,1) - starting_locations(i,1)).^2 + ...
+        (G.nodes(:,2) - starting_locations(i,2)).^2;
+    [~,near_node] = min(dists);
+    starting_verts(i) = near_node;
+end
+
+node_list = zeros(ceil(max_dist/env_step_size)*tau,1);
+last_node_list = zeros(size(node_list,1),N);
+time_index = zeros(ceil(max_dist/env_step_size)*tau,1);
+V = cell(N,1);
+% set up starting locations
+for i=1:N
+    V{i} = table(node_list, time_index); % create table within each cell
+    V{i}{1,:} = [starting_verts(i), 1];
+end
+c_row = 2*ones(N,1);
+connected_configs = zeros(floor(T/T_I),N);
+t = 1;
+
+while t <= floor(T/T_I)
+    if v_level
+        fprintf("horizon %d of %d ... \n",t,floor(T/T_I));
+    end
+    iter = 1;
+    while ~converged_flag
+        if v_level
+            fprintf("iteration %d\n",iter);
+        end
+        for i = randperm(N)
+            
+            % reset path for robot k
+            V{i}{c_row(i):end,:} = 0;
+            % enumerate some feasible paths
+            [~,path] = dijkstra(G,getVerts(V,i,'end'),max_dist);
+            % remove those paths disconnected at T_I
+            path_trimmed = cell(size(path));
+            for j=1:length(path)
+                if ~isnan(path{j})
+                    % if its a real path, check connectivity
+                    verts = union(path{j}(end), getVerts(V,setdiff(1:N,i),'end'));
+                    connected_flag = checkConnected(Gc,verts);
+                    % remove those disconnected paths
+                    if connected_flag
+                        path_trimmed{j} = path{j};
+                    end
+                end
+            end
+            
+            path_trimmed = path_trimmed(~cellfun('isempty',path_trimmed));
+            max_util = 0;
+            max_idx = -1;
+            for j=1:length(path_trimmed)
+                [existing_verts, ~] = getVerts(V,1:N,'all');
+                
+                verts = [existing_verts; path_trimmed{j}']; 
+                util = trajectoryUtility(verts, G, env, sig);
+                % find maximum utility
+                if util > max_util
+                    max_util = util;
+                    max_idx = j;
+                end
+            end
+
+            if max_idx == -1
+                if v_level
+                    fprintf("shite\n");
+                end
+                utility = -1;
+                tmp = toc(t_start);
+                computation_time = -1;
+                path_length = -1;
+                return
+            end
+            best_path = path_trimmed{max_idx};
+            
+            % update the path taken for robot i
+            next_idx = find(V{i}.node_list,1,'last');
+            time_idx = V{i}.time_index(next_idx);
+            V{i}.node_list(next_idx:(next_idx+length(best_path)-1)) = best_path;
+            V{i}.time_index(next_idx:(next_idx+length(best_path)-1)) = ...
+                time_idx:(time_idx+length(best_path)-1);
+            
+        end
+        
+        iter = iter + 1;
+        converged_flag = true;
+        for i=1:N % check all path's convergence
+            if ~all(V{i}.node_list == last_node_list(:,i))
+                % if not all are equal, then we're not converged
+                converged_flag = false;
+            end
+            last_node_list(:,i) = V{i}.node_list; % update shift register
+        end
+        
+    end
+    
+    % update rows in V and reset convergence flag and track connected
+    % configs
+    for i=1:N
+        c_row(i) = find(V{i}.node_list,1,'last')+1;
+        connected_configs(t,i) = getVerts(V,i,'end');
+    end
+    
+    converged_flag = false;
+    
+    pos = G.nodes(connected_configs(t,:),1:2);
+    goals = goal_locations;
+    if all(sqrt((pos(:,1) - goals(:,1)).^2 + (pos(:,2) - goals(:,2)).^2) < rho)
+        goals_reached = true;
+    end
+    
+    t = t + 1;
+    
+end
+
+verts = getVerts(V,1:N,'all');
+utility = trajectoryUtility(verts, G, env, sig);
+computation_time = toc(t_start);
+path_length = 0;
+for i=1:N
+    D = dist(G.nodes(getVerts(V,i,'all'),1:2)');
+    path_length = path_length + sum(diag(D(:,2:end)));
+end
+
+%% plotting
+
+if show_plot
+    % plot again after each agent re-plans
+    figure(2); clf; hold on;
+    colormap parula;
+    imagesc(environment_size(1):0.1:environment_size(2),...
+        environment_size(3):0.1:environment_size(4),Z);
+    colors = hsv(N);
+    for n=1:N
+        plot(G.nodes(nonzeros(V{n}.node_list),1), G.nodes(nonzeros(V{n}.node_list),2),...
+            '.', 'color', colors(n,:), 'markersize', 20,'linestyle','-');
+        for j=1:length(V{n}.node_list)
+            if V{n}.node_list(j) ~= 0
+                text(G.nodes(nonzeros(V{n}.node_list(j)),1), G.nodes(nonzeros(V{n}.node_list(j)),2), num2str(j))
+            end
+        end
+        plot(G.nodes(V{n}.node_list(1),1), G.nodes(V{n}.node_list(1),2),...
+            'd','color',colors(n,:),'markersize',12,'linewidth', 2);
+        plot(G.nodes(nonzeros(connected_configs(:,n)),1), ...
+            G.nodes(nonzeros(connected_configs(:,n)),2), ...
+            'd','color',colors(n,:),'markersize',12,'linewidth', 2);
+        plot(goal_locations(n,1), goal_locations(n,2), '^',...
+            'color',colors(n,:),'markersize',12,'linewidth',2)
+    end
+    %     plot(G.nodes(:,1), G.nodes(:,2), 'k.','markersize',10);
+    plot(obstacle_points(:,1), obstacle_points(:,2), 'r.','markersize',16,'linewidth',3,'markerfacecolor','r');
+    axis(environment_size);
+end
+

HS/monteCarloRunner.m

+% monte carlo path planning runner
+% HOLLINGER AND SINGH
+clear all; close all; clc;
+% set up random number generator
+
+M = 20;
+m = 1;
+offset = 1;
+xdata = linspace(0.15,0.10,M);
+util = zeros(M,1);
+comp_time = zeros(M,1);
+
+while m <= M
+    rng(m + offset);
+    fprintf("\nbegin iteration %d --------- \n",m);
+    input_env_step_size = xdata(m);
+    [utility, computation_time] = main(input_env_step_size, m);
+    if utility ~= -1 && computation_time ~= -1
+        util(m) = utility;
+        comp_time(m) = computation_time;
+        m = m + 1;
+    else
+        fprintf("\niteration %d failed \n",m);
+        offset = offset + 1;
+    end
+end
+time_str = datestr(now,31);
+time_str = regexprep(time_str(6:end-3),':','-');
+time_str = regexprep(time_str,' ','_');
+filename = ['HS_allrand_M',num2str(M),'_',time_str,'.mat'];
+save(filename,'util','comp_time','xdata');
+
+offset
+m
\ No newline at end of file

HS/trajectoryUtility.m

+function util = trajectoryUtility(verts, G, env, sig)
+
+% Tj = size(plcs,2);
+% [vertags,vertplcs] = covertIdxForm(verts,Tj);
+% agentplcs = zeros(length(verts),2);
+% % get each vertex position
+% for a = 1:length(verts)
+%     agentplcs(a,:) = plcs{k,vertplcs(a)}(vertags(a),:);
+% end
+
+
+coords = G.nodes(verts,1:2);
+util = 0;
+
+for i=1:length(env.x_area)
+    for j=1:length(env.y_area)
+        dists = sqrt((coords(:,1) - env.x_area(i)).^2 + (coords(:,2) - env.y_area(j)).^2);
+        if any(dists <= sig) % if anybody is close enough to get a measurement
+            util = util + env.Z(j, i);
+        end
+    end
+end
+
+% experiment with a horizontal additive element to the objective function
+% max_Z = max(max(env.Z));
+% min_Z = min(min(env.Z));
+% 
+% if strcmp(option, 'dists')
+%     mean_dist = norm(mean(goals,1) - mean(coords,1));
+%     beta = max(env.x_area);
+%     h_gain = (1/beta)*abs(beta - mean_dist);
+%     alpha = 10;
+%     util = util + alpha*h_gain;
+% elseif strcmp(option, 'goals')
+%     N = size(goals,1);
+%     close_dist = zeros(N,1);
+%     for i=1:N
+%         coords_i = coords(order == i,:);
+%         close_dist(i) = norm(coords_i(end,:) - goals(i,:));
+%     end
+%     
+%     beta = max(env.x_area);
+%     h_dist = sum(close_dist);
+%     normalizer = (1/(1 + N*beta));
+%     h_gain = (1/(1 + h_dist))/normalizer;
+%     alpha = t/3; % testing a time dependent solution
+%     util = util + alpha*h_gain;
+%     
+% end
+
+end
\ No newline at end of file

IIP/IIP_fig_gen.m

+
+
+xdata = [floor(linspace(200,1200,M))', linspace(0.15,0.1,M)'];
+input_N = 3;
+v_level = 1;
+input_pj = 100;
+m = -1;
+[utility, computation_time, path_length] = multi_agent_path_planning(input_pj, m, v_level, input_N)
\ No newline at end of file

IIP/RRT_base.m

+clear all;
+% close all;
+clc;
+
+% basic RRT program with obstacles, will integrate with sampling-based intermittent
+% interaction code later
+
+show_live_plot = false;
+show_end_plot = true;
+env = [0, 20, 0, 20]; % environment [xmin, xmax, ymin, ymax]
+xdim = env(2) - env(1);
+ydim = env(4) - env(3);
+dist_inc = 0.5; % incremental distance
+init_conf = [10,1]; % initial configuration [x, y]
+final_conf = [14,18]; % final configuration [x, y]
+dx = 0.1;
+star_radius = 5; % radius to look for rewire points
+threshold = 0.35;
+safe_radius = 2;
+
+[x_obs, y_obs] = generateObstacles(env, threshold, dx, init_conf, final_conf, safe_radius);
+
+obs_points = [x_obs, y_obs];
+xlist = dx:dx:xdim;
+ylist = dx:dx:ydim;
+if show_live_plot
+    figure(1); clf; hold on;
+    plot(x_obs, y_obs, '.k')
+end
+
+% now for the RRT stuff
+K = 2000; % start with 1000 vertices in graph
+G = zeros(K);
+verts = zeros(K,2); % coords of all points
+verts(1,:) = init_conf;
+% verts(2,:) = final_conf;
+
+for k=3:K
+    
+    % generate random config
+    q_rand = [rand*xdim + env(1), rand*ydim + env(3)];
+    
+    % locate nearest vertex
+    q_near = getNearestVertex(q_rand, verts, -1); % just want one point
+    q_new = getNewConfig(q_near, q_rand, dist_inc, obs_points, dx);
+    if isempty(q_new)
+        continue;
+    end
+%     cmin = norm(q_new - q_near); % cost to get from nearest point to new point
+    
+    % we have a working new configuation, add it and an edge
+    % check for new neighbors too
+    near_index = find(all(verts == q_near,2));
+    verts(k,:) = q_new;
+    G(near_index, k) = 1;
+    G = min(G + G',1);
+    if norm(final_conf - q_new) <= dist_inc
+        fprintf("final configuration has been reached\n")
+        verts(2,:) = final_conf;
+        G(2,k) = 1;
+        G(k,2) = 1;
+        break;
+    end
+    
+    
+    % progress updates
+    if mod(k,100) == 0
+        fprintf("iteration %d\n", k);
+    end
+    if show_live_plot
+        plot(q_new(1), q_new(2), '.b')
+        plot([q_near(1), q_new(1)], [q_near(2), q_new(2)], 'g-')
+        drawnow;
+    end
+    
+end
+
+% generate shortest path
+gr = graph(G);
+vert_path = shortestpath(gr,1,2);
+if show_end_plot
+    figure(1); clf; hold on;
+    plot(x_obs, y_obs, '.k')
+    plot(verts(vert_path,1), verts(vert_path,2), 'mo-')
+end
+
+% dual tree stuff:
+
+%     dists = sqrt((q_new(1) - verts(:,1)).^2 + (q_new(2) - verts(:,2)).^2);
+%     new_neighbors = find(dists <= dist_inc); % adding neighbors like this is kinda of bullshit? we need to check for bloackages
+%     new_neighbors = new_neighbors(new_neighbors ~= near_index);
+
+%     G(near_index, new_neighbors) = 1;
+
+%         for i=1:length(new_neighbors)
+%             plot([verts(new_neighbors(i),1), q_new(1)], [verts(new_neighbors(i),2), q_new(2)], 'g-')
+%         end
\ No newline at end of file

IIP/RRT_star.m

+clear all;
+% close all;
+clc;
+
+% basic RRT program with obstacles, will integrate with sampling-based intermittent
+% interaction code later
+
+show_live_plot = false;
+show_end_plot = true;
+env = [0, 20, 0, 20]; % environment [xmin, xmax, ymin, ymax]
+xdim = env(2) - env(1);
+ydim = env(4) - env(3);
+dist_inc = 2; % incremental distance
+init_conf = [10,1]; % initial configuration [x, y]
+final_conf = [14,18]; % final configuration [x, y]
+dx = 0.1;
+star_radius = dist_inc; % radius to look for rewire points
+threshold = 0.35;
+safe_radius = 2;
+
+[x_obs, y_obs] = generateObstacles(env, threshold, dx, init_conf, final_conf, safe_radius);
+
+obs_points = [x_obs, y_obs];
+xlist = dx:dx:xdim;
+ylist = dx:dx:ydim;
+if show_live_plot
+    figure(1); clf; hold on;
+    plot(x_obs, y_obs, '.k')
+end
+
+% now for the RRT stuff
+K = 1000; % start with 1000 vertices in graph
+G = zeros(K);
+verts = zeros(K,4); % coords of all points and theyre costs and parents [x coord, y coord, cost to go, parent]
+verts(1,:) = [init_conf, 0, 1];
+% verts(2,:) = final_conf;
+
+for k=3:K
+    
+    % generate random config
+    q_rand = [rand*xdim + env(1), rand*ydim + env(3)];
+    
+    % locate nearest vertex
+    q_near = getNearestVertex(q_rand, verts, -1); % just want one point
+    q_new = getNewConfig(q_near(1:2), q_rand, dist_inc, obs_points, dx);
+    if isempty(q_new)
+        continue;
+    end
+    % got a new configuration, update cost to go and parent information
+    near_index = find(all(verts == q_near,2));
+    q_new = [q_new, q_near(3) + norm(q_new-q_near(1:2)), near_index];
+    
+    cmin = q_new(3); % cost to get from nearest point to new point
+    ind_min = -1;
+    nearest_vertices = getNearestVertex(q_new, verts, star_radius);
+    for i=1:size(nearest_vertices,1)
+        % itrerate through nearest neighbors to look for easier method
+        [collision_flag, end_point] = checkCollisions(q_new(1:2), nearest_vertices(i,1:2), dx, obs_points);
+        new_cost = (nearest_vertices(i,3)+norm(nearest_vertices(i,1:2) - q_new(1:2)));
+        if ~collision_flag && new_cost < cmin
+            cmin = new_cost; % new lowest cost source vertex
+            ind_min = i; % index of new lowest cost source vertex
+        end
+    end
+    if ind_min ~= -1
+        new_parent = find(all(verts == nearest_vertices(ind_min,:),2));
+        if new_parent ~= 2 % never add final config as a parent node, dont make no sense
+            q_new(3) = cmin; % update cost and parent
+            q_new(4) = new_parent;
+        end
+    end
+    
+    rewired_list = zeros(size(nearest_vertices,1),1); % list of vertices that get rewired to use q_new
+    for i=1:size(nearest_vertices,1)
+        
+        % look for lower cost if q_new was parent
+        new_cost = q_new(3) + norm(q_new(1:2) - nearest_vertices(i,1:2));
+        % check collisions
+        [collision_flag, ~] = checkCollisions(q_new(1:2), nearest_vertices(i,1:2), dx, obs_points);
+        if new_cost < nearest_vertices(i,3) && ~collision_flag
+            % we need to update the parent of this vert
+            rewired_vert = find(all(verts == nearest_vertices(i,:),2));
+            verts(rewired_vert,4) = k;
+            verts(rewired_vert,3) = new_cost; % update cost to get there too
+            rewired_list(i) = rewired_vert;
+        end
+        
+    end
+    
+    verts(k,:) = q_new;
+    G(near_index, k) = 1;
+    G = min(G + G',1);
+    if norm(final_conf - q_new(1:2)) <= dist_inc
+%         fprintf("final configuration has been reached\n")
+        verts(2,:) = [final_conf, norm(final_conf - q_new(1:2)), k];
+        G(2,k) = 1;
+        G(k,2) = 1;
+%         break;
+    end
+    
+    
+    % progress updates
+    if mod(k,100) == 0
+        fprintf("iteration %d\n", k);
+    end
+    if show_live_plot
+        plot(q_new(1), q_new(2), '.b')
+        plot([verts(q_new(4),1), q_new(1)], [verts(q_new(4),2), q_new(2)], 'g-')
+        % also plot line to original nearest vertex
+        plot([q_near(1), q_new(1)], [q_near(2), q_new(2)], 'r:')
+        % also also plot rewired neighbors (somehow)
+        rewired_list = rewired_list(rewired_list ~= 0); % clear out zeros
+        for i=1:length(rewired_list)
+            plot([verts(rewired_list(i),1), q_new(1)], [verts(rewired_list(i),2), q_new(2)], 'b:')
+            plot(verts(rewired_list(i),1), verts(rewired_list(i),2), 'b*')
+        end
+        axis(env);
+        drawnow;
+    end
+    
+%     if any(verts(:,4) == 2)
+%         fprintf("F\n")
+%     end
+    
+end
+
+%% plotting
+% generate shortest path
+% gr = graph(G);
+% vert_path = shortestpath(gr,1,2);
+if show_end_plot
+    figure(1); clf; hold on;
+    plot_vert = 2;
+    while true
+        % follow final config backward to starting config
+        fprintf("next vert: %d\n", plot_vert);
+        plot(verts(plot_vert,1), verts(plot_vert,2), 'mo');
+        xs = [verts(plot_vert,1), verts(verts(plot_vert,4),1)];
+        ys = [verts(plot_vert,2), verts(verts(plot_vert,4),2)];
+        plot(xs, ys, 'm-');
+        if plot_vert == 1
+            break;
+        end
+        plot_vert = verts(plot_vert,4); % go back to parent
+    end
+    plot(x_obs, y_obs, '.k')
+    plot(final_conf(1), final_conf(2), '*r')
+    plot(init_conf(1), init_conf(2), '*g')
+end
+
+% dual tree stuff:
+
+%     dists = sqrt((q_new(1) - verts(:,1)).^2 + (q_new(2) - verts(:,2)).^2);
+%     new_neighbors = find(dists <= dist_inc); % adding neighbors like this is kinda of bullshit? we need to check for bloackages
+%     new_neighbors = new_neighbors(new_neighbors ~= near_index);
+
+%     G(near_index, new_neighbors) = 1;
+
+%         for i=1:length(new_neighbors)
+%             plot([verts(new_neighbors(i),1), q_new(1)], [verts(new_neighbors(i),2), q_new(2)], 'g-')
+%         end
\ No newline at end of file

IIP/buildGMM.m

+function [gmPDF, Z, gm, x_area, y_area] = buildGMM(plotArea, mu, sigma)
+% BUILDGMM builds a gmm based on means and comvariances and gives the pdf
+% back along with a matrix of points evaluated over the sim plot range
+
+% mu = 10*rand(k,sim.d); % means
+% sigma = 5 + ones(1,2,k); % covariances
+gm = gmdistribution(mu, sigma);
+gmPDF = @(x,y)pdf(gm,[x y]);
+x_area = plotArea(1):0.1:plotArea(2); % use plot range
+y_area = plotArea(3):0.1:plotArea(4);
+Z = zeros(numel(y_area), numel(x_area));
+for i = 1:numel(x_area)
+    for j = 1:numel(y_area)
+        
+        Z(j,i) = gmPDF(x_area(i), y_area(j));
+        
+    end
+end
+
+% want to build in clever sampling based on the gm distribution so we dont
+% have to sample as much
+
+end
\ No newline at end of file

IIP/checkCollisions.m

+function [collision_flag, end_point] = checkCollisions(q_start, q_end, dx, obs_points)
+
+step_num = ceil(norm(q_start - q_end)./dx); % number of steps of dx to get to the distance increment
+vec = (q_end - q_start)./norm(q_start - q_end);
+vec = vec.*dx;
+pt = q_start;
+collision_flag = false;
+
+for i=1:step_num
+    
+    % look for collisions (there is a faster way to do this, i am sure)
+    if ~isempty(obs_points)
+        dists = sqrt((pt(1) - obs_points(:,1)).^2 + (pt(2) - obs_points(:,2)).^2);
+    else
+        dists = [];
+    end
+    if any(dists <= dx)
+        % collision!
+        collision_flag = true;
+        end_point = pt;
+        return;
+    end
+    
+    % otherwise take a step
+    pt = pt + vec;
+    
+end
+
+end_point = pt;
+
+end
\ No newline at end of file

IIP/conncompAdj.m

+function [S,C] = conncompAdj(G)
+[p,q,r] = dmperm(G'+speye(size(G)));
+S = numel(r)-1;
+C = cumsum(full(sparse(1,r(1:end-1),1,1,size(G,1))));
+C(p) = C;
+% C = C-1; % return comps - 1
+S = S - 1;
+end
\ No newline at end of file

Matlab Code/generateGreedySolutionFast.m → IIP/generateGreedySolutionFast.m

@@ -7,15 +7,15 @@ score = zeros(1,Tk);
 
 if parOpt
     if reqConn
-        fprintf("selecting parallelized constrained samples\n")
+%         fprintf("selecting parallelized constrained samples\n")
     else
-        fprintf("selecting parallelized unconstrained samples\n")
+%         fprintf("selecting parallelized unconstrained samples\n")
     end
 else
     if reqConn
-        fprintf("selecting serialized constrained samples\n")
+%         fprintf("selecting serialized constrained samples\n")
     else
-        fprintf("selecting serialized unconstrained samples\n")
+%         fprintf("selecting serialized unconstrained samples\n")
     end
 end
 
@@ -43,7 +43,7 @@ if reqConn
                     continue; % if connectivity is required, can only pick samples from within the constrained bounding set
                 end
                 
-                util = trajectoryUtility([verts;i], plcs, env, k, sig, plotArea); % dont care about connectivity here
+                util = trajectoryUtilityIIP([verts;i], plcs, env, k, sig, plotArea); % dont care about connectivity here
                 if util > maxUtil
                     maxUtil = util;
                     nextNode = i;
@@ -57,13 +57,13 @@ if reqConn
             end
         end
         if ~isempty(verts)
-            score(k) = trajectoryUtility(verts, plcs, env, k, sig, plotArea);
+            score(k) = trajectoryUtilityIIP(verts, plcs, env, k, sig, plotArea);
             plcSel{k} = verts;
         else
             score(k) = 0;
             plcSel{k} = [];
         end
-        fprintf("samples selected for time slice %d\n", k);
+%         fprintf("samples selected for time slice %d\n", k);
     end
     
 else % connectivity is not required
@@ -81,7 +81,7 @@ else % connectivity is not required
                 if length(unique(ags)) < length(ags) % if multiple agents are being used
                     continue;
                 end
-                util = trajectoryUtility([verts;i], plcs, env, k, sig, plotArea); % dont care about connectivity here
+                util = trajectoryUtilityIIP([verts;i], plcs, env, k, sig, plotArea); % dont care about connectivity here
                 if util > maxUtil
                     maxUtil = util;
                     nextNode = i;
@@ -92,9 +92,9 @@ else % connectivity is not required
                 verts = [verts;nextNode];
             end
         end
-        score(k) = trajectoryUtility(verts, plcs, env, k, sig, plotArea);
+        score(k) = trajectoryUtilityIIP(verts, plcs, env, k, sig, plotArea);
         plcSel{k} = verts;
-        fprintf("samples selected for time slice %d\n", k);
+%         fprintf("samples selected for time slice %d\n", k);
     end
     
 end

Matlab Code/getConstBoundSet.m → IIP/getConstBoundSet.m

@@ -10,12 +10,21 @@ r = rho/2;
 for i=1:length(x_area)
     for j=1:length(y_area)
         if inpolygon(x_area(i), y_area(j), bddBox(:,1), bddBox(:,2))
-            obs_dists = sqrt((obs_points(:,1) - x_area(i)).^2 + (obs_points(:,2) - y_area(j)).^2);
-            if ~any(obs_dists <= 0.2)
-                if Z(j,i) >= maxVal
-                    maxVal = Z(j,i);
-                    maxX = x_area(i);
-                    maxY = y_area(j);
+            th = 0:0.1:(2*pi);
+            [xSet, ySet] = pol2cart(th,r);
+            tmp_set = [xSet'+x_area(i),ySet'+y_area(j)];
+            if all(inpolygon(tmp_set(:,1), tmp_set(:,2), bddBox(:,1), bddBox(:,2)))
+                if ~isempty(obs_points)
+                    obs_dists = sqrt((obs_points(:,1) - x_area(i)).^2 + (obs_points(:,2) - y_area(j)).^2);
+                else
+                    obs_dists = [];
+                end
+                if ~any(obs_dists <= 0.2)
+                    if Z(j,i) >= maxVal
+                        maxVal = Z(j,i);
+                        maxX = x_area(i);
+                        maxY = y_area(j);
+                    end
                 end
             end
         end

IIP/load_params.m

+% big-ass parameter file
+% INTERMITENT INTERACTION PLANNING
+% sample selection information
+Tk = 3;
+K = 1; 
+Pj = 200;
+rho = 0.5;
+sig = 0.25;
+environment_size = [0 5 0 5];
+
+% environmental heatmap info
+mu = [environment_size(1) + rand(3,1)*environment_size(2), ...
+      environment_size(3) + rand(3,1)*environment_size(4)];
+gmmMeans = size(mu,1);
+sigma = 3*ones(1,2,gmmMeans);
+sigma(:,:,1) = [5 5];
+sigma(:,:,2) = [5 5];
+
+% multi-agent path planning information
+collision_radius = 0.1;
+env_step_size = 0.2; % (upper bound)
+es = environment_size;
+sb = [es(1)+0.1, es(1)+0.9, es(3)+0.1, es(4)-0.1];
+gb = [es(2)-0.9, es(2)-0.1, es(3)+0.1, es(4)-0.1];
+
+connected_flag = false;
+collision_flag = false;
+while collision_flag || ~connected_flag
+    starting_locations = [sb(1) + (sb(2) - sb(1))*rand(N,1), sb(3) + (sb(4) - sb(3))*rand(N,1)];
+    collision_flag = any(nonzeros(dist(starting_locations')) <= collision_radius,'all');
+    adj = dist(starting_locations') <= rho - (env_step_size)/2;
+    adj = adj - diag(diag(adj));
+    [~,C] = conncompAdj(adj);    
+    if all(C == 1)
+        connected_flag = true;
+    else
+        connected_flag = false;
+    end
+end
+
+connected_flag = false;
+collision_flag = false;
+while collision_flag || ~connected_flag
+    goal_locations = [gb(1) + (gb(2) - gb(1))*rand(N,1), gb(3) + (gb(4) - gb(3))*rand(N,1)];
+    collision_flag = any(nonzeros(dist(goal_locations')) <= collision_radius,'all');
+    adj = dist(goal_locations') <= rho - (env_step_size)/2;
+    adj = adj - diag(diag(adj));
+    [~,C] = conncompAdj(adj);
+    if all(C == 1)
+        connected_flag = true;
+    else
+        connected_flag = false;
+    end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%% temp
+starting_locations = [0.5 3;
+                      0.15 3.4;
+                      0.5 3.8];
+goal_locations = [4.5 1;
+                  4.85 1.4;
+                  4.5 1.8];
+%%%%%%%%%%%%%%%%%%%%%%%% temp
+                  
+token_ownership = true;
+threshold = 0.4;
+safe_radius = 0.6;
+step_size = 0.1;
+move_speed = 1;
+planning_iterations = 300;
+max_sample_itr = 10^6;
+distance_increment = 1;
+
+% build environmental heatmap
+[gmPDF, Z, gmObj, x_area, y_area] = buildGMM(environment_size, mu, sigma);
+env.Z = Z;
+env.x_area = x_area;
+env.y_area = y_area;
+env.gmObj = gmObj;
+
+% plotting parameters
+lw = 1.5;
+fs = 16;
+ms = 10;
+show_plot = true;
+show_plot_env = false;
+
+% Hollinger parameters
+obs_step_size = 0.1;
+move_size = distance_increment; 
+T_I = 2; % interval of disconnection
+max_dist = T_I*move_speed;
+T = (Tk+1)*T_I; % total horizon length
+tau = T/T_I;
+converged_flag = false;
+goals_reached = false;

IIP/monteCarloGroup.m

+% group monte carlo runner
+clear all; close all; clc;
+
+addpath('C:\Users\Larkin\Google Drive\DropBox\VT\Research\SAR\Interaction Planning\MATLAB\Hollinger_Singh');
+
+M = 20;
+A = 10;
+m = 15;
+offset = 294;
+xdata = [floor(linspace(200,800,M))', linspace(0.2,0.1,M)'];
+seeds = zeros(M,A);
+util = zeros(M,2*A); % [[IIP_1 IIP_2 ... IIP_A], [HS_1 HS_2 ... HS_A]]
+comp_time = zeros(M,2*A);
+path_lengths = zeros(M,2*A);
+
+while m <= M
+    fprintf("\nbegin iteration %d with offset %d --------- \n",m,offset);
+    input_pj = xdata(m,1);
+    input_env_step_size = xdata(m,2);
+    
+    a = 1;
+    while a <= A
+        fprintf("\naveraging iteration %d, offset %d --------- \n",a,offset);
+        rng(offset);
+        [util(m,a), comp_time(m,a), path_lengths(m,a)] = multi_agent_path_planning(input_pj, m);
+        if util(m,a) == -1 || comp_time(m,a) == -1
+            fprintf("\niteration %d, run %d, has failed \n",m,a);
+            offset = offset + 1;
+            continue;
+        end
+        
+        rng(offset);
+        [util(m,A+a), comp_time(m,A+a), path_lengths(m,A+a)] = main(input_env_step_size, m);
+        if util(m,A+a) == -1 || comp_time(m,A+a) == -1
+            fprintf("\niteration %d, run %d, has failed \n",m,a);
+            offset = offset + 1;
+            continue;
+        end
+        
+        seeds(m,a) = offset;
+        a = a + 1;
+        offset = offset + 1;
+    end
+    
+    m = m + 1;
+end
+time_str = datestr(now,31);
+time_str = regexprep(time_str(6:end-3),':','-');
+time_str = regexprep(time_str,' ','_');
+filename = ['averaged_p2_M',num2str(M),'_',time_str,'.mat'];
+save(filename,'util','comp_time','xdata','seeds','A','M','path_lengths');
+
+offset
+m
\ No newline at end of file

IIP/monteCarloParallel.m

+% group monte carlo runner
+clear all; close all; clc;
+
+addpath('C:\Users\Larkin\Google Drive\DropBox\VT\Research\SAR\Interaction Planning\MATLAB\Hollinger_Singh');
+
+M = 10;
+A = 42; % should be divisable by N
+m = 1;
+N = 3;
+% assume upper bound of 10 fails per iteration
+j = 10*M*(A/N);
+v_level = 0; % quiet mode
+
+xdata = [floor(linspace(200,1200,M))', linspace(0.15,0.1,M)'];
+% xdata = [600*ones(M,1), 0.1*ones(M,1)];
+Ndata = N*ones(M,1);
+if length(Ndata) ~= M || length(xdata) ~= M
+    error('check ya self son');
+end
+seedsLAB = zeros(M,A);
+utilLAB = zeros(M,2*A); % [[IIP_1 IIP_2 ... IIP_A], [HS_1 HS_2 ... HS_A]]
+comp_timeLAB = zeros(M,2*A);
+path_lengthsLAB = zeros(M,2*A);
+
+seeds = zeros(M,A);
+util = zeros(M,2*A);
+comp_time = zeros(M,2*A);
+path_lengths = zeros(M,2*A);
+
+% parpool(N);
+
+start_time = tic;
+
+spmd
+
+    n = labindex;
+    offset = n*j;
+
+    m = 1;
+    while m <= M
+
+        fprintf("\nworker %d beginning iteration %d with offset %d --------- \n",n,m,offset);
+        input_pj = xdata(m,1);
+        input_env_step_size = xdata(m,2);
+        input_N = Ndata(m);
+
+        a = (n-1)*(A/N) + 1;
+        a_stop = n*(A/N);
+
+        while a <= a_stop
+            fprintf("\nworker %d averaging iteration %d, offset %d --------- \n",n,a,offset);
+            rng(offset);
+            [utilLAB(m,a), comp_timeLAB(m,a), path_lengthsLAB(m,a)] = multi_agent_path_planning(input_pj, m, v_level, input_N);
+            if utilLAB(m,a) == -1 || comp_timeLAB(m,a) == -1
+                fprintf("\niteration %d, run %d, has failed \n",m,a);
+                offset = offset + 1;
+                continue;
+            end
+            
+            rng(offset);
+            [utilLAB(m,A+a), comp_timeLAB(m,A+a), path_lengthsLAB(m,A+a)] = main(input_env_step_size, m, v_level, input_N);
+            if utilLAB(m,A+a) == -1 || comp_timeLAB(m,A+a) == -1
+                fprintf("\niteration %d, run %d, has failed \n",m,a);
+                offset = offset + 1;
+                continue;
+            end
+            
+            seedsLAB(m,a) = offset;
+            a = a + 1;
+            offset = offset + 1;
+        end
+        m = m + 1;
+        
+    end
+
+end
+
+stop_time = toc(start_time);
+
+% re-assemble matrices
+for n=1:N
+    a = (n-1)*(A/N) + 1;
+    a_stop = n*(A/N);
+    
+    tmp = seedsLAB{n};
+    seeds(:,a:a_stop) = tmp(:,a:a_stop);
+    tmp = utilLAB{n};
+    util(:,a:a_stop) = tmp(:,a:a_stop);
+    util(:,(a:a_stop)+A) = tmp(:,(a:a_stop)+A);
+    tmp = comp_timeLAB{n};
+    comp_time(:,a:a_stop) = tmp(:,a:a_stop);
+    comp_time(:,(a:a_stop)+A) = tmp(:,(a:a_stop)+A);
+    tmp = path_lengthsLAB{n};
+    path_lengths(:,a:a_stop) = tmp(:,a:a_stop);
+    path_lengths(:,(a:a_stop)+A) = tmp(:,(a:a_stop)+A);
+end
+
+time_str = datestr(now,31);
+time_str = regexprep(time_str(6:end-3),':','-');
+time_str = regexprep(time_str,' ','_');
+filename = ['averaged_M',num2str(M),'_A',num2str(A),'_',time_str,'.mat'];
+save(filename,'util','comp_time','xdata','Ndata','seeds','A','M','path_lengths','stop_time');
+
+offset
+m
+stop_time
+

IIP/monteCarloRunner.m

+% monte carlo path planning runner
+% INTERMITENT INTERACTION PLANNING
+clear all; close all; clc;
+% set up random number generator
+
+M = 20;
+m = 1;
+offset = 1;
+xdata = floor(linspace(150,250,M));
+util = zeros(M,1);
+comp_time = zeros(M,1);
+
+while m <= M
+    rng(m + offset);
+    fprintf("\nbegin iteration %d --------- \n",m);
+    input_pj = xdata(m);
+    [utility, computation_time] = multi_agent_path_planning(input_pj, m);
+    if utility ~= -1 && computation_time ~= -1
+        util(m) = utility;
+        comp_time(m) = computation_time;
+        m = m + 1;
+    else
+        fprintf("\niteration %d failed \n",m);
+        offset = offset + 1;
+    end
+end
+time_str = datestr(now,31);
+time_str = regexprep(time_str(6:end-3),':','-');
+time_str = regexprep(time_str,' ','_');
+filename = ['IIP_allrand_M',num2str(M),'_',time_str,'.mat'];
+save(filename,'util','comp_time','xdata');
+
+offset
+m
\ No newline at end of file

IIP/multi_agent_path_planning.m

+function [utility, computation_time, path_length] = multi_agent_path_planning(input_pj, m, v_level, input_N)
+
+N = input_N;
+load_params;
+
+Pj = input_pj;
+% obstacle stuff
+% [x_pts, y_pts] = generateObstacles(environment_size, threshold, obs_step_size, starting_locations, goal_locations, safe_radius);
+% obstacle_points = [x_pts, y_pts];
+
+load('obstacle_saved_easy','obstacle_points');
+obstacle_points(obstacle_points(:,1) >= 5,:) = [];
+obstacle_points(:,1) = obstacle_points(:,1) - 1.5;
+obstacle_points(:,2) = obstacle_points(:,2) - 2;
+
+dummy = rrtAgent(N, mean(starting_locations,1), mean(goal_locations,1), token_ownership, ...
+    environment_size, obstacle_points, distance_increment, step_size, ...
+    planning_iterations, move_speed, collision_radius, v_level);
+
+t_start = tic;
+
+dummy = dummy.planMultiGoalPath;
+if isempty(dummy.path_waypoints)
+    utility = -1;
+    computation_time = -1;
+    path_length = -1;
+    tmp = toc(t_start);
+    return
+end
+
+% figure(1); clf; hold on;
+% plot(obstacle_points(:,1), obstacle_points(:,2), 'k.');
+% plot(starting_locations(:,1), starting_locations(:,2), 'bo');
+% plot(goal_locations(:,1), goal_locations(:,2), 'bo');
+
+t_int = (dummy.path_len)./(dummy.move_speed*(Tk+1));
+if t_int < 2*sig
+    
+    if v_level
+        fprintf('not enough room to generate constrained bounding sets\n');
+    end
+    
+    utility = -1;
+    computation_time = -1;
+    path_length = -1;
+    tmp = toc(t_start);
+    return
+end
+sample_centers = zeros(Tk,2); % center of sampling bounding areas
+sample_slopes = zeros(Tk,1);
+for k=1:(Tk)
+    sample_centers(k,:) = dummy.timeLookup(k*t_int, {dummy.path_waypoints});
+    tmp_pt = dummy.timeLookup(k*t_int - 0.01, {dummy.path_waypoints});
+    sample_slopes(k) = (tmp_pt(2) - sample_centers(k,2))/(tmp_pt(1) - sample_centers(k,1));
+end
+
+plcs = cell(Tk,Pj);
+sample_sets = cell(Tk,1);
+sample_sets_const = cell(Tk,1);
+
+% bounding sets
+w = t_int;
+h = 1.5; %%%%%%%%%%%%%%%%%%%%%%%%%temp
+a = atan(w/h);
+r = (w/2)/sin(a);
+
+for k=1:Tk
+    th = atan(sample_slopes(k)) + [pi/2 - a, pi/2 + a, (3*pi/2)- a, (3*pi/2) + a];
+    [c_pt_x,c_pt_y] = pol2cart(th,r);
+    sample_sets{k} = [sample_centers(k,1) + c_pt_x', sample_centers(k,2) + c_pt_y'];
+    sample_sets{k} = [sample_sets{k}; sample_sets{k}(1,:)];
+    
+    [c_pt_x_tmp,c_pt_y_tmp] = pol2cart(th,r - rho/2); % bound away from sides of set
+    bdd_set_tmp = [sample_centers(k,1) + c_pt_x_tmp', sample_centers(k,2) + c_pt_y_tmp'];
+    sample_sets_const{k} = getConstBoundSet(Z,x_area,y_area,bdd_set_tmp,rho, obstacle_points); % bounding set for tight sampling case
+    
+    % generate samples for this time slice
+    x_len = max(sample_sets{k}(:,1)) - min(sample_sets{k}(:,1));
+    y_len = max(sample_sets{k}(:,2)) - min(sample_sets{k}(:,2));
+    x_min = min(sample_sets{k}(:,1));
+    y_min = min(sample_sets{k}(:,2));
+
+    counter = 1;
+    sample_itr = 0;
+    while counter <= Pj && sample_itr < max_sample_itr
+        tmp_pt = [x_min + x_len*rand(N,1), y_min + y_len*rand(N,1)];
+        collision_flag = dummy.checkObstaclesOnly(tmp_pt, sample_centers(k,:));
+        if all(inpolygon(tmp_pt(:,1), tmp_pt(:,2), sample_sets{k}(:,1), sample_sets{k}(:,2))) && ~collision_flag
+            plcs{k,counter} = tmp_pt;
+            counter = counter + 1;
+        end
+        sample_itr = sample_itr + 1;
+    end
+    if isempty(plcs{k,end})
+        if v_level
+            fprintf('ran out of sample iterations, bailing\n')
+        end
+        utility = -1;
+        computation_time = -1;
+        path_length = -1;
+        tmp = toc(t_start);
+        return
+    end
+    const_empty_flag = false(N,1);
+    for j=1:Pj
+        const_empty_flag = or(inpolygon(plcs{k,j}(:,1), plcs{k,j}(:,2), ...
+            sample_sets_const{k}(:,1), sample_sets_const{k}(:,2)), const_empty_flag);
+    end
+    if ~all(const_empty_flag)
+        if v_level
+            fprintf('insufficient sampling, constrained sets empty, bailing\n')
+        end
+        utility = -1;
+        computation_time = -1;
+        path_length = -1;
+        tmp = toc(t_start);
+        return
+    end
+end
+
+% select the samples based on coverage environmental heatmap
+[plcs_sel, gdy_interaction_times] = selectSamples(sample_sets_const, plcs, rho, sig, N, env, K);
+
+% plan paths for all agents
+agents = cell(N,1);
+agent_paths = cell(N,1);
+agent_pts = [];
+
+for i=1:N
+    agent_goals = [plcs_sel{i};goal_locations(i,:)];
+    agents{i} = rrtAgent(N, starting_locations(i,:), agent_goals, token_ownership, ...
+        environment_size, obstacle_points, distance_increment, step_size, ...
+        planning_iterations, move_speed, collision_radius);
+    agents{i}.id = i;
+end
+
+for k=1:Tk+1
+    time_obstacles = cell(N,1);
+    for i=1:N
+        agents{i}.time_obs = time_obstacles;
+        agents{i} = agents{i}.planMultiGoalPath;
+        if agents{i}.path_failed
+            utility = -1;
+            computation_time = -1;
+            path_length = -1;
+            tmp = toc(t_start);
+            return
+        end
+        time_obstacles{i} = agents{i}.path_waypoints;
+        agent_paths{i} = [agent_paths{i};agents{i}.path_waypoints];
+        agent_pts = [agent_pts;agents{i}.path_waypoints];
+    end
+end
+
+% evaluate performance:
+% verts in Tj*N
+utility = pointTrajectoryUtility(agent_pts, env, sig);
+computation_time = toc(t_start);
+path_length = 0;
+for i=1:N
+    path_length = path_length + agents{i}.path_len;
+end
+
+%% plotting stuff
+
+if show_plot
+    figure(1); clf; hold on; grid on;
+    set(gcf, 'Position',  [200, 200, 600, 600])
+    colormap parula;
+    my_grey = [0.7 0.7 0.7];
+    if show_plot_env
+    imagesc(environment_size(1):0.1:environment_size(2),...
+        environment_size(3):0.1:environment_size(4),Z);
+    end
+    colors = hsv(N);
+    plot(dummy.obs_points(:,1), dummy.obs_points(:,2), 'k.', 'markersize', ms+8, 'linewidth', lw);
+    for a=1:N
+        sl_handle = plot(starting_locations(a,1), starting_locations(a,2), 's', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
+        gl_handle = plot(goal_locations(a,1), goal_locations(a,2), 'd', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
+    end
+    core_handle = plot(dummy.path_waypoints(:,1), dummy.path_waypoints(:,2), 'm.-', 'markersize', ms, 'linewidth', lw);
+    for k=1:Tk
+        for j=1:Pj
+            plot(plcs{k,j}(:,1), plcs{k,j}(:,2), '.', 'markersize', ms, 'linewidth', lw, ...
+                'markerfacecolor',my_grey, 'markeredgecolor',my_grey);
+        end
+        for i=1:N
+            plot(plcs_sel{i}(k,1), plcs_sel{i}(k,2), '.', 'markersize', ms+8, 'linewidth', lw, ...
+                'markerfacecolor',colors(i,:), 'markeredgecolor',colors(i,:));
+        end
+        bdd_box_handle = plot(sample_sets{k}(:,1), sample_sets{k}(:,2), 'k--', 'markersize', ms, 'linewidth', lw);
+        cnst_box_handle = plot(sample_sets_const{k}(:,1), sample_sets_const{k}(:,2), 'k:', 'markersize', ms, 'linewidth', lw);
+    end
+    
+    for i=1:N
+        t = 0.1:0.02:(agents{i}.path_len/agents{i}.move_speed);
+        for j=1:length(t)
+            pt = agents{i}.timeLookup(t(j), {agent_paths{i}});
+            plot(pt(1), pt(2), '.', 'markersize', ms, 'linewidth', lw, ...
+                'markerfacecolor',colors(i,:), 'markeredgecolor',colors(i,:));
+        end
+    end
+    legend([core_handle,bdd_box_handle,cnst_box_handle,sl_handle,gl_handle],'Core path','$B_k$','$\overline{B_k}$',...
+        'Start','Goal','Location','best','Interpreter','Latex');
+    axis image
+    axis(environment_size)
+    set(gca,'FontSize',fs)
+
+end
+
+end

IIP/perlin_noise.m

+function im = perlin_noise(im)
+
+    [n, m] = size(im);
+    i = 0;
+    w = sqrt(n*m);
+
+    while w > 3
+        i = i + 1;
+        d = interp2(randn(n, m), i-1, 'spline');
+        im = im + i * d(1:n, 1:m);
+        w = w - ceil(w/2 - 1);
+    end
\ No newline at end of file

IIP/pointTrajectoryUtility.m

+function util = pointTrajectoryUtility(pts, env, sig)
+
+util = 0;
+
+for i=1:length(env.x_area)
+    for j=1:length(env.y_area)
+        dists = sqrt((pts(:,1) - env.x_area(i)).^2 + (pts(:,2) - env.y_area(j)).^2);
+        if any(dists <= sig) % if anybody is close enough to get a measurement
+            util = util + env.Z(j, i);
+        end
+    end
+end
+
+end
\ No newline at end of file

Matlab Code/selectSamples.m → IIP/selectSamples.m

-function [plcs_sel, gdyInteractionTimes] = selectSamples(sample_sets_const, plcs)
+function [plcs_sel, gdyInteractionTimes] = selectSamples(sample_sets_const, plcs, rho, sig, N, env, K)
 
 % select samples given a set of viable samples, a heatmap, and both types
 % of bounding sets
 
-load_params; % reload all known parameters
+% load_params; % reload all known parameters
 
 % booleans
 gdySolutions = true;
 parOpt = false;
-
-env.Z = Z;
-env.x_area = x_area;
-env.y_area = y_area;
-env.gmObj = gmObj;
+environment_size = [env.x_area(1), env.x_area(end), env.y_area(1), env.y_area(end)];
+Tk = size(plcs,1);
+Pj = size(plcs,2);
 
 supergraphs = generateSupergraphs(plcs,rho,false);
 
@@ -26,12 +24,12 @@ if gdySolutions
     for k=1:Tk
         % check both solutions for validity
         if length(greedy_sol_const{k}) < N || length(greedy_sol_unconst{k}) < N
-            error('we failed to effectively select greedy samples, insufficient samples probably');
+%             error('we failed to effectively select greedy samples, insufficient samples probably');
         end
     end
 end
 %---------------------------------------------%
-fprintf('--------------------------------\n')
+% fprintf('--------------------------------\n')
 
 % assume we can get the optimal solution, somehow. Now to compare trade
 % offs:

IIP/test_obstacle_generator.m

+clear all;
+clc;
+
+
+threshold = 0.45;
+safe_radius = 1;
+% step_size = 0.1;
+env_step_size = 0.1;
+
+starting_locations = [1,2;1 2.5;1 3];
+goal_locations = [9.5,2;9.5 2.5;9.5 3];
+environment_size = [0 10 0 5];
+
+% [x_pts, y_pts] = generateObstacles(environment_size, threshold, env_step_size, starting_locations, goal_locations, safe_radius);
+center_points = [7 2;
+                 3.75 3.75];
+obstacle_radius = 1.0;
+
+% [X,Y] = meshgrid((environment_size(1)-obstacle_radius):env_step_size:(obstacle_radius+environment_size(2)), ...
+%     (environment_size(3)-obstacle_radius):env_step_size:(environment_size(4)+obstacle_radius));
+[X,Y] = meshgrid((environment_size(1)):env_step_size:(environment_size(2)), ...
+    (environment_size(3)):env_step_size:(environment_size(4)));
+x_pts = X(:);
+y_pts = Y(:);
+obstacle_points = zeros(length(x_pts),2);
+
+for i=1:size(center_points,1)
+    init_d = sqrt((center_points(i,1) - x_pts).^2 + (center_points(i,2) - y_pts).^2);
+    x_pts_tmp = x_pts(init_d <= obstacle_radius);
+    y_pts_tmp = y_pts(init_d <= obstacle_radius);
+    last_idx = nnz(obstacle_points(:,1));
+    obstacle_points(last_idx+1:(last_idx+length(x_pts_tmp)),:) = [x_pts_tmp, y_pts_tmp];
+end
+
+nonzeros_x_obstacle = nonzeros(obstacle_points(:,1));
+nonzeros_y_obstacle = nonzeros(obstacle_points(:,2));
+obstacle_points = [nonzeros_x_obstacle, nonzeros_y_obstacle];
+
+
+% [X,Y] = meshgrid(4:env_step_size:6,0:env_step_size:5);
+% obstacle_points = [X(:),Y(:)];
+
+% for i=1:size(starting_locations,1)
+%     init_d = sqrt((starting_locations(i,1) - x_pts).^2 + (starting_locations(i,2) - y_pts).^2);
+%     x_pts = x_pts(init_d >= safe_radius);
+%     y_pts = y_pts(init_d >= safe_radius);
+% end
+% for i=1:size(goal_locations,1)
+%     final_d = sqrt((goal_locations(i,1) - x_pts).^2 + (goal_locations(i,2) - y_pts).^2);
+%     x_pts = x_pts(final_d >= safe_radius);
+%     y_pts = y_pts(final_d >= safe_radius);
+% end
+
+ms = 8;
+lw = 2;
+fs = 14;
+figure(1); clf; hold on;
+plot(obstacle_points(:,1), obstacle_points(:,2), 'k.', 'markersize', ms, 'linewidth', lw);
+
+axis image
+axis(environment_size)
+set(gca,'FontSize',fs)
+
+% save('obstacle_saved_easy.mat','obstacle_points');
\ No newline at end of file

IIP/time_param_testing.m

+clear all; clc;
+
+dt = 0.1;
+waypoints = [1 1;2 2;3 4;6 3;3 6;9 9]; % waypoints to move through
+move_speed = 1; % movement speed of uav
+waypoint_len = size(waypoints,1);
+D = dist(waypoints');
+dist_list = diag(D(1:end-1,2:end));
+total_dist = sum(dist_list);
+total_time = total_dist/move_speed;
+t = dt:dt:total_time; % time parameter
+
+% now we need to determine, based on time, what set of waypoints we are
+% passing between
+
+figure(1); clf; hold on;
+
+for k=1:length(t)
+
+    q_time = t(k); % time we are interested in evaluating
+    q_dist = q_time*move_speed;
+    dist_list_sum = [0;cumsum(dist_list)];
+    for i=1:length(dist_list_sum)
+        if q_dist <= dist_list_sum(i)
+            wp_index = i;
+            break;
+        end
+    end
+
+    unit_vec = waypoints(wp_index,:) - waypoints(wp_index - 1,:);
+    unit_vec = unit_vec/norm(unit_vec);
+    q_point = waypoints(wp_index - 1,:) + unit_vec*(q_dist - dist_list_sum(wp_index - 1));
+    plot(q_point(1), q_point(2), 'k.');
+
+end
\ No newline at end of file

Matlab Code/trajectoryUtility.m → IIP/trajectoryUtilityIIP.m

-function util = trajectoryUtility(verts, plcs, env, k, sig, plotArea)
+function util = trajectoryUtilityIIP(verts, plcs, env, k, sig, plotArea)
 
 Tj = size(plcs,2);
 [vertags,vertplcs] = covertIdxForm(verts,Tj);

Matlab Code/load_params.m

-% big-ass parameter file
-
-% sample selection information
-Tk = 3; % time slices
-K = 2; % constraint for interaction
-Pj = 50; % samples per time slice
-rho = 0.75;
-sig = 0.5;
-
-% environmental heatmap info
-mu = [1 1;3 1;8 3];
-gmmMeans = size(mu,1);
-sigma = 3*ones(1,2,gmmMeans);
-sigma(:,:,1) = [5 5];
-sigma(:,:,2) = [5 5];
-
-% multi-agent path planning information
-agent_number = 3;
-N = agent_number;
-starting_locations = [1,2;1 2.5;1 3];
-% goal_locations = [5,5;9,9];
-goal_final = [9,2;9 2.5;9 3];
-goal_locations = goal_final;
-token_ownership = true;
-environment_size = [0 10 0 5];
-threshold = 0.40;
-safe_radius = 1;
-step_size = 0.1;
-collision_radius = 0.5;
-move_speed = 1;
-planning_iterations = 300;
-max_sample_itr = 50*Pj; % get average 50 misses per sample point (?)
-distance_increment = 3;
-% [x_pts, y_pts] = generateObstacles(environment_size, threshold, step_size, starting_locations, goal_locations, safe_radius);
-% obstacle_points = [x_pts, y_pts];
-
-load("obstacle_saved.mat");
-
-% build environmental heatmap
-[gmPDF, Z, gmObj, x_area, y_area] = buildGMM(environment_size, mu, sigma);
-
-% plotting parameters
-lw = 1.5; % line width
-fs = 16; % font size
-ms = 10; % marker size
\ No newline at end of file

Matlab Code/multi_agent_path_planning.m

-clear all; clc;
-% multi agent path planning testing, including
-% - adaptive sampling based on core safe path (tbd)
-% - sample selection via sequential greedy methods (tbd, copy paste)
-% - multi-agent path planning based on selected samples (d)
-% - randomized obstacles based on perlin noise (d)
-
-load_params;
-% plan an initial safe path to build samples around
-
-dummy = rrtAgent(agent_number, mean(starting_locations,1), mean(goal_final,1), token_ownership, ...
-                environment_size, obstacle_points, distance_increment, step_size, ...
-                planning_iterations, move_speed, collision_radius);
-            
-dummy = dummy.planMultiGoalPath;
-% dummy.path_verts
-% dummy.path_waypoints
-if isempty(dummy.path_waypoints)
-    error('failed to find a core path to goal');
-end
-
-% build sample areas around regular intervals of path
-
-t_int = (dummy.path_len)./(dummy.move_speed*(Tk+1)); % interval between time slices (dont need one at goal)
-sample_centers = zeros(Tk,2); % center of sampling bounding areas
-for k=1:(Tk)
-    sample_centers(k,:) = dummy.timeLookup(k*t_int, {dummy.path_waypoints});
-end
-
-% take the samples and make sure they have a clear view of core path
-plcs = cell(Tk,Pj); % cell array for placements per time slice
-sample_sets = cell(Tk,1);
-sample_sets_const = cell(Tk,1);
-
-% set up circular bounding sets
-r = t_int*dummy.move_speed/2;
-th = 0:0.05:(2*pi);
-
-for k=1:Tk
-    [c_pt_x,c_pt_y] = pol2cart(th,r);
-    sample_sets{k} = [sample_centers(k,1) + c_pt_x', sample_centers(k,2) + c_pt_y'];
-    
-    [c_pt_x_tmp,c_pt_y_tmp] = pol2cart(th,r - rho/2); % bound away from sides of set
-    bdd_set_tmp = [sample_centers(k,1) + c_pt_x_tmp', sample_centers(k,2) + c_pt_y_tmp'];
-    sample_sets_const{k} = getConstBoundSet(Z,x_area,y_area,bdd_set_tmp,rho, obstacle_points); % bounding set for tight sampling case
-    
-    % generate samples for this time slice
-    x_len = max(sample_sets{k}(:,1)) - min(sample_sets{k}(:,1));
-    y_len = max(sample_sets{k}(:,2)) - min(sample_sets{k}(:,2));
-    x_min = min(sample_sets{k}(:,1));
-    y_min = min(sample_sets{k}(:,2));
-    counter = 1;
-    sample_itr = 0;
-    while counter <= Pj && sample_itr < max_sample_itr
-        % guided random sampling, put box around 
-        tmp_pt = [x_min + x_len*rand(N,1), y_min + y_len*rand(N,1)];
-        % check for collisions with obstacles!
-        collision_flag = dummy.checkObstaclesOnly(tmp_pt, sample_centers(k,:));
-        if all(inpolygon(tmp_pt(:,1), tmp_pt(:,2), sample_sets{k}(:,1), sample_sets{k}(:,2))) && ~collision_flag
-            plcs{k,counter} = tmp_pt;
-            counter = counter + 1;
-        end
-        sample_itr = sample_itr + 1;
-    end
-    if isempty(plcs{k,end})
-        error('ran out of sample iterations')
-    end
-end
-
-% select the samples based on coverage environmental heatmap
-[plcs_sel, gdy_interaction_times] = selectSamples(sample_sets_const, plcs);
-
-figure(1); clf; hold on;
-colors = hsv(N);
-plot(dummy.obs_points(:,1), dummy.obs_points(:,2), 'k.', 'markersize', ms, 'linewidth', lw);
-for a=1:agent_number
-    plot(starting_locations(a,1), starting_locations(a,2), 'd', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
-    plot(goal_locations(a,1), goal_locations(a,2), 'd', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
-end
-plot(dummy.path_waypoints(:,1), dummy.path_waypoints(:,2), 'go-', 'markersize', ms, 'linewidth', lw);
-for k=1:Tk
-    for j=1:Pj
-        plot(plcs{k,j}(:,1), plcs{k,j}(:,2), 'b.', 'markersize', ms, 'linewidth', lw);
-    end
-    plot(sample_sets{k}(:,1), sample_sets{k}(:,2), 'r--', 'markersize', ms, 'linewidth', lw);
-    plot(sample_sets_const{k}(:,1), sample_sets_const{k}(:,2), 'c--', 'markersize', ms, 'linewidth', lw);
-end
-for i=1:N
-%     plot(plcs_sel{i}(:,1), plcs_sel{i}(:,2), 'o','color', colors(i,:), 'markersize', ms, 'linewidth', lw, 'markerfacecolor',colors(i,:));
-end
-axis image
-axis(environment_size)
-set(gca,'FontSize',fs)
-% key point, we are ignoring the intermediate goal positions because we
-% assume that the core safe path, which DOES go through the intermediate
-% goals, guides the overall path correctly. (should not be a big deal)
-agents = cell(N,1);
-time_obstacles = cell(N,1);
-for i=1:N
-    agent_goals = [plcs_sel{i};goal_final(i,:)]; % take only final goal position for agent 1
-    agents{i} = rrtAgent(agent_number, starting_locations(i,:), agent_goals, token_ownership, ...
-                    environment_size, obstacle_points, distance_increment, step_size, ...
-                    planning_iterations, move_speed, collision_radius);
-    agents{i}.id = i;
-    agents{i}.time_obs = time_obstacles; % this is some decentralization violation stuff here
-    agents{i} = agents{i}.planMultiGoalPath;
-    time_obstacles{i} = agents{i}.path_waypoints;
-end
-
-%% plotting stuff
-
-figure(2); clf; hold on;
-mymap = [linspace(1, 0.35, 150); linspace(1, 0.35, 150); linspace(1, 0.35, 150)]';
-colormap parula;
-hplt = imagesc(environment_size(1):0.1:environment_size(2),environment_size(3):0.1:environment_size(4),...
-Z);
-plot(obstacle_points(:,1), obstacle_points(:,2), 'k.', 'markersize', ms, 'linewidth', lw);
-
-
-dt = 0.1;
-t_max = ceil(1.2*d_max/agents{1}.move_speed);
-if mod(t_max,2) ~= 0
-    t_max = t_max+1;
-end
-t = dt:dt:(t_max);
-t_len = length(t);
-
-my_map = [linspace(0,1,t_len)', linspace(1,0,t_len)', linspace(0,0,t_len)'];
-
-% plot colors indicating time along the path
-time_count = 1; % which time slice we have reached so far
-a = 1; % which agent we are working on
-agent_reach_flag = zeros(N,1); % which agents have reached the  current time slice, needs resetting
-agent_times = ones(N,1);
-color_idx = 1;
-color_start = 1;
-color_end = 1;
-itr_flag = true;
-clc;
-while time_count < Tk+1
-    while itr_flag
-        if a == 1
-%             color_idx
-        end
-        % plot agent points
-        pt = agents{a}.timeLookup(t(agent_times(a)), {agents{a}.path_waypoints});
-        plot(pt(1), pt(2), '.','color', my_map(color_idx,:), 'markersize', ms+8, 'linewidth', lw);
-        if time_count == Tk
-            tmp_dist = norm(pt - goal_locations(a,:));
-        else
-            tmp_dist = norm(pt - plcs_sel{a}(time_count,:));
-        end
-        if tmp_dist < step_size % if we are close to sample
-            a = a + 1;
-            color_end = max(color_idx, color_end); % record new high point
-            color_idx = color_start; % reset color parameter
-        end
-        if a > N
-            color_start = color_end;
-            color_idx = color_start;
-            a = 1;
-            break;
-        end
-        agent_times(a) = agent_times(a) + 1;
-        color_idx = color_idx + 1;
-    end
-    time_count = time_count + 1;
-    itr_flag = true;
-end
-
-for k=1:Tk
-    for a=1:N
-        plot(plcs_sel{a}(k,1), plcs_sel{a}(k,2), 'o', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
-    end
-end
-
-d_max = -1;
-total_len = 0;
-for a=1:agent_number
-    plot(starting_locations(a,1), starting_locations(a,2), 'd', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
-    plot(goal_locations(a,1), goal_locations(a,2), 'd', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
-%     plot(agents{a}.path_waypoints(2:end-1,1), agents{a}.path_waypoints(2:end-1,2), 's-', 'color',colors(a,:),...
-%         'markersize', ms, 'linewidth', lw);
-    
-    % find longest waypoint path
-%     D = dist(obj.path_waypoints');
-%     waypoint_lens = diag(D(1:end-1,2:end));
-%     max([A, B],[],2)
-    if d_max < agents{a}.path_len
-        d_max = agents{a}.path_len;
-    end
-    total_len = total_len + agents{a}.path_len;
-end
-
-% my_map = [linspace(0,1,t_len)', linspace(1,0,t_len)', [linspace(0,1,t_len/2) linspace(1,0,t_len/2)]'];
-
-for k=1:Tk
-    if ismember(k,gdy_interaction_times)
-        plot(sample_sets_const{k}(:,1), sample_sets_const{k}(:,2), 'c--', 'markersize', ms, 'linewidth', lw);
-    end
-end
-
-plot(starting_locations(:,1), starting_locations(:,2), '.','color', my_map(1,:), 'markersize', ms+8, 'linewidth', lw);
-
-% for a=1:agent_number
-%     for i=1:(length(agents{a}.goal_list) - 1)
-%         plot(agents{a}.goal_list{i}.coords(1), agents{a}.goal_list{i}.coords(2), '*', 'color', colors(a,:),...
-%             'markersize', ms, 'linewidth', lw);
-%     end
-% end
-% plot time obstacles
-% for i=1:length(agent.time_obs)
-% plot(agent.time_obs{i}(:,1),agent.time_obs{i}(:,2),'r*--');
-% end
-axis image
-axis(environment_size)
-set(gca,'FontSize',fs)
-drawnow;
-
-% waypoints = [1 1;2 2;4 7];
-% agent.timeLookup(0.001,waypoints)
-
-% okay so now we need to take samples around a core path
\ No newline at end of file