Upload New 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