Skip to content

M
Multi-agent interaction planning
Commit 5fe17651 authored by Larkin Heintzman's avatar Larkin Heintzman
Browse files
Options

Upload New File

parent 0a37e09d
Pipeline #33 canceled with stages
      Hide whitespace changes
      Inline Side-by-side
      Showing with 448 additions and 0 deletions
      Matlab Code/rrtAgent.m 0 → 100644
      View file @ 5fe17651
      classdef rrtAgent
      properties
      N
      id
      start
      goal_list
      goal
      path
      path_verts
      path_waypoints
      path_len
      verts
      token
      dx
      collision_radius
      dist_inc
      env
      obs_points
      time_obs
      move_speed
      planning_itr
      vert_num
      show_live_plot
      show_end_plot
      end
      methods
      function obj = rrtAgent(agent_number, starting_location, goal_locations, token_ownership, ...
      environment_size, obstacle_points, distance_increment, step_size, ...
      planning_iterations, move_speed, collision_radius)
      % initializer of agent
      if nargin < 4
      error("missing some information")
      else
      if length(goal_locations) < 1
      error("no goal information!")
      end
      end
      obj.N = agent_number;
      obj.start = starting_location;
      obj.planning_itr = planning_iterations;
      obj.goal_list = cell(size(goal_locations,1),1);
      for i=1:size(goal_locations,1)
      obj.goal_list{i} = struct('coords',goal_locations(i,:),'parent',-1,'cost',Inf,'index',obj.planning_itr+i);
      % assume sequential visiting!
      end
      obj.goal = struct('coords',goal_locations(1,:),'parent',-1,'cost',Inf,'index',obj.planning_itr+1);
      obj.path = {}; % vertex indexs to follow for a path
      obj.path_verts = {};
      obj.vert_num = obj.planning_itr;
      obj.verts = cell(obj.planning_itr,1); % all vertex sample information
      obj.verts{1} = struct('parent',[],'coords',obj.start,'index',1,'cost',0); % preloaded w info
      obj.token = token_ownership;
      obj.dx = step_size;
      obj.collision_radius = collision_radius;
      obj.dist_inc = distance_increment;
      obj.env = environment_size; % [xmin xmax ymin ymax]
      obj.obs_points = obstacle_points; % obstacle points (blocked points)
      obj.time_obs = {};
      % obj.time_obs{1} = [9 9;8 8;8 5;5 5;4 5;4 3;1 1]; % time parameterized obstacles
      % obj.time_obs{2} = rot90([9 9;8 8;8 5;5 5;4 5;4 3;2 2],2);
      % obj.time_obs{1} = [1 2;8 9]; % time parameterized obstacles
      % obj.time_obs{2} = [2 1;9 8];
      obj.move_speed = move_speed;
      % temp plotting parameters
      obj.show_live_plot = false;
      obj.show_end_plot = false;
      end
      function obj = planMultiGoalPath(obj)
      % plan a path that hits all the goals in goal_locations
      for k=1:length(obj.goal_list)
      obj.goal = obj.goal_list{k}; % update goal position
      if k ~= 1
      obj.start = obj.goal_list{k-1}.coords;
      end
      obj = obj.resetSamples;
      fprintf("Agent %d planning path %d/%d...\n", obj.id,k,length(obj.goal_list));
      obj = obj.planPath; % generate path to selected goal position
      if ~isempty(obj.path)
      path_tmp = fliplr(obj.path);
      for i=1:length(obj.path)
      obj.path_verts{end+1} = obj.verts{path_tmp{i}}; % append all vertices from path
      end
      obj.path_verts{end+1} = obj.goal;
      else
      fprintf("no path found\n");
      end
      end
      % scan path vertices into waypoint list
      obj.path_waypoints = zeros(length(obj.path_verts),2);
      for i=1:length(obj.path_verts)
      % plot(obj.path_verts{i}.coords(1), obj.path_verts{i}.coords(2), 'mo');
      obj.path_waypoints(i,:) = obj.path_verts{i}.coords;
      end
      D = dist(obj.path_waypoints');
      obj.path_len = sum(diag(D(1:end-1,2:end)));
      end
      function intercept_time = timeIntersect(obj, waypoints)
      % checks the waypoint list provided with the known time
      % obstacles (other agents' waypoint lists)
      intercept_time = -1;
      D = dist(waypoints');
      dist_list = diag(D(1:end-1,2:end));
      total_dist = sum(dist_list);
      total_time = total_dist/obj.move_speed;
      t = (obj.dx*obj.move_speed):(obj.dx*obj.move_speed):total_time; % time parameter
      % now we need to determine, based on time, what set of waypoints we are
      % passing between
      q_time = (obj.dx*obj.move_speed); % start query time
      while q_time <= total_time
      q_point_waypoint = obj.timeLookup(q_time, {waypoints});
      q_point_obstacle = obj.timeLookup(q_time, obj.time_obs);
      if size(q_point_waypoint,1) > 1
      error('waypoint-based point should be single point!');
      end
      dists = sqrt((q_point_waypoint(:,1) - q_point_obstacle(:,1)).^2 ...
      + (q_point_waypoint(:,2) - q_point_obstacle(:,2)).^2);
      min_dist = min(dists); % minimum distance from point to obstacle
      if min_dist <= obj.collision_radius
      intercept_time = q_time; % collision!
      break;
      else
      % slightly questionable
      q_time = q_time + 0.90*(min_dist./(2*obj.move_speed)); % move forward in time based on distance
      % hopefully this will keep us from overshooting the
      % actual collision point because we are moving time
      % forward assuming agents travel in a straight line
      % toward one another
      end
      end
      end
      function q_point = timeLookup(obj, q_time, waypoints)
      % need a way to convert times, and lists of waypoints, into a
      % 2D point(s) in space
      if ~iscell(waypoints)
      error('waypoints must be cell array')
      end
      if ~all(cellfun(@isempty,waypoints))
      q_point = zeros(length(waypoints),2);
      for j=1:length(waypoints)
      wpts_tmp = waypoints{j};
      if isempty(wpts_tmp)
      continue; % skip empty waypoints
      end
      D = dist(wpts_tmp');
      dist_list = diag(D(1:end-1,2:end));
      q_dist = q_time*obj.move_speed;
      dist_list_sum = [0;cumsum(dist_list)];
      wp_index = -1;
      for i=1:length(dist_list_sum)
      if q_dist <= dist_list_sum(i)
      wp_index = i;
      break;
      end
      end
      if wp_index ~= -1
      unit_vec = wpts_tmp(wp_index,:) - wpts_tmp(wp_index - 1,:);
      unit_vec = unit_vec/norm(unit_vec);
      q_point(j,:) = wpts_tmp(wp_index - 1,:) + unit_vec*(q_dist - dist_list_sum(wp_index - 1));
      else
      q_point(j,:) = wpts_tmp(end,:); % assume that the agent stops at end
      end
      end
      else
      q_point = zeros(0,2);
      end
      end
      function obj = resetSamples(obj)
      % resets all path planning variables so that we can replan from
      % a new location without accidentally reusing samples
      obj.verts = cell(obj.planning_itr,1); % reset position samples
      obj.path = {};
      obj.verts{1} = struct('parent',[],'coords',obj.start,'index',1,'cost',0); % preloaded w info
      end
      function obj = planPath(obj)
      % dO tHiNgS!!
      if obj.show_live_plot
      figure(1); clf; hold on;
      plot(obj.obs_points(:,1), obj.obs_points(:,2), '.k');
      axis(obj.env);
      end
      if obj.show_end_plot
      figure(2); clf; hold on;
      plot(obj.obs_points(:,1), obj.obs_points(:,2), '.k');
      axis(obj.env);
      end
      for k=2:obj.planning_itr
      % generate random point
      xdim = obj.env(2) - obj.env(1);
      ydim = obj.env(4) - obj.env(3);
      pt_rand = [rand*xdim + obj.env(1), rand*ydim + obj.env(3)];
      vert_near = getNearestVertex(obj, pt_rand, -1); % vert_near nearest vertex
      vert_new = getNewConfig(obj, vert_near, pt_rand, k); % pick from vert near and generate new config w min cost
      % check if valid
      if isempty(vert_new)
      continue;
      end
      % hope this shit does not break anything important
      obj.verts{k} = vert_new; % update vertex list w new config
      obj = obj.rewireNeighbors(vert_new);
      % examine goal positions
      goal_dist = norm(obj.goal.coords - vert_new.coords);
      [collision_flag, ~] = obj.checkCollisions(vert_new, obj.goal);
      if goal_dist < obj.dist_inc && ~collision_flag
      % can touch goal location
      if (goal_dist + vert_new.cost) < obj.goal.cost
      obj.goal.cost = goal_dist + vert_new.cost;
      obj.goal.parent = vert_new.index;
      end
      end
      % progress updates:
      if mod(k,100) == 0
      fprintf("iteration: %d\n", k);
      end
      % plot things
      if obj.show_live_plot
      figure(1);
      xs = [vert_new.coords(1), obj.verts{vert_new.parent}.coords(1)];
      ys = [vert_new.coords(2), obj.verts{vert_new.parent}.coords(2)];
      plot(xs, ys, 'b.--');
      axis(obj.env);
      drawnow;
      end
      end
      if obj.goal.parent == -1
      fprintf("failed to make it to goal position\n");
      else
      if obj.show_end_plot
      figure(2);
      plot(obj.goal.coords(1,1), obj.goal.coords(1,2), 'r*');
      plot(obj.start(1,1), obj.start(1,2), 'g*');
      plot_vert = obj.goal.parent;
      plot([obj.goal.coords(1) obj.verts{plot_vert}.coords(1)], [obj.goal.coords(2) obj.verts{plot_vert}.coords(2)], 'b.--');
      while true
      fprintf("next vertex to plot: %d\n", plot_vert);
      if plot_vert == 1
      xs = obj.verts{1}.coords(1);
      ys = obj.verts{1}.coords(2);
      else
      xs = [obj.verts{plot_vert}.coords(1), obj.verts{obj.verts{plot_vert}.parent}.coords(1)];
      ys = [obj.verts{plot_vert}.coords(2), obj.verts{obj.verts{plot_vert}.parent}.coords(2)];
      end
      plot(xs, ys, 'b.--');
      if plot_vert == 1
      break;
      end
      plot_vert = obj.verts{plot_vert}.parent;
      end
      axis(obj.env);
      end
      next_vert = obj.goal.parent;
      c = 1;
      while true
      obj.path{c} = next_vert;
      if next_vert == 1
      break;
      end
      next_vert = obj.verts{next_vert}.parent;
      c = c + 1;
      end
      end
      end
      function ancestors = lineageLookup(obj, vert_start)
      % look up all parents of starting vertex and compose into a
      % list
      ancestors = {};
      ancestors{end+1} = vert_start; % start with starting vertex of course
      next_vert = vert_start.parent;
      while true
      if isempty(next_vert)
      break;
      else
      ancestors{end+1} = obj.verts{next_vert};
      next_vert = obj.verts{next_vert}.parent; % continue iterating
      end
      end
      end
      function collision_flag = checkObstaclesOnly(obj, pts_sample, pt_goal)
      collision_flag = false;
      for i=1:size(pts_sample,1)
      pt = pts_sample(i,:);
      total_dist = norm(pt - pt_goal);
      for j=1:ceil(total_dist./obj.dx)
      vec = (pt_goal - pt)./norm(pt_goal - pt);
      dists = sqrt((obj.obs_points(:,1) - pt(1)).^2 + (obj.obs_points(:,2) - pt(2)).^2);
      if any(dists <= obj.dx)
      collision_flag = true;
      end
      pt = pt + obj.dx.*vec; % walk towards sample center
      end
      end
      end
      function [collision_flag, stop_point] = checkCollisions(obj, vert_start, vert_end)
      % do things
      if isstruct(vert_start)
      pt_start = vert_start.coords;
      else
      pt_start = vert_start;
      end
      if isstruct(vert_end)
      pt_end = vert_end.coords;
      else
      pt_end = vert_end;
      end
      total_dist = norm(pt_end - pt_start);
      step_num = ceil(min(obj.dist_inc,total_dist)./obj.dx); % number of steps req.
      vec = ((pt_end - pt_start)./total_dist).*obj.dx;
      pt = pt_start;
      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) - obj.obs_points(:,1)).^2 + (pt(2) - obj.obs_points(:,2)).^2);
      if any(dists <= obj.dx)
      % collision!
      collision_flag = true;
      break;
      end
      % otherwise take a step
      pt = pt + vec;
      end
      stop_point = pt;
      % build waypoint list so we can check possible time collisions
      if isstruct(vert_start)
      vert_list = obj.lineageLookup(vert_start);
      waypoints = zeros(length(vert_list)+1,2);
      waypoints(1,:) = stop_point;
      for i=1:length(vert_list)
      waypoints(i+1,:) = vert_list{i}.coords;
      end
      waypoints = flipud(waypoints);
      else
      error('vertex starting position should be a struct!');
      end
      intercept_time = obj.timeIntersect(waypoints);
      if intercept_time ~= -1
      collision_flag = true;
      end
      end
      function vert_near = getNearestVertex(obj, pt_test, search_radius)
      % find nearest set of points within search radius, if search
      % radius is -1, we only want the nearest point
      dmin = Inf;
      if search_radius == -1
      for i=1:obj.vert_num
      if isempty(obj.verts{i})
      continue;
      end
      tmp_dist = norm(pt_test - obj.verts{i}.coords);
      if tmp_dist < dmin
      dmin = tmp_dist;
      vert_near = obj.verts{i};
      end
      end
      else
      vert_near = {};
      c = 1;
      for i=1:obj.vert_num
      if isempty(obj.verts{i})
      continue;
      end
      tmp_dist = norm(pt_test - obj.verts{i}.coords);
      if tmp_dist < search_radius
      vert_near{c} = obj.verts{i};
      c = c + 1; % inc counter
      end
      end
      end
      end
      function new_config = getNewConfig(obj, vert_near, pt_rand, iteration)
      % compare near neighbors and pick cheapest one to be parent
      if length(vert_near) > 1
      error("list is too long!")
      end
      [~, stop_point] = obj.checkCollisions(vert_near, pt_rand); % not looking for collision, but new point
      new_cost = vert_near.cost + norm(vert_near.coords - stop_point);
      new_config = struct('parent',vert_near.index,'coords',stop_point,'index',iteration,'cost',new_cost);
      % now look through close vertices to see if they would be
      % cheaper parents
      new_parent = vert_near.index;
      cmin = new_config.cost;
      verts_near = obj.getNearestVertex(new_config.coords, obj.dist_inc); % this time its a list
      for i=1:length(verts_near)
      [collision_flag, ~] = obj.checkCollisions(verts_near{i}, new_config); % only looking for collision
      if ~collision_flag
      tmp_cost = verts_near{i}.cost + norm(verts_near{i}.coords - new_config.coords);
      if tmp_cost < cmin
      cmin = tmp_cost;
      new_parent = verts_near{i}.index; % save new cost and index of new parent
      end
      end
      end
      % update with new information
      new_config.parent = new_parent;
      new_config.cost = cmin;
      end
      function obj = rewireNeighbors(obj, new_config)
      % rewire nearby vertices with new configuration as parent
      verts_near = obj.getNearestVertex(new_config.coords, obj.dist_inc); % this time its a list
      for i=1:length(verts_near)
      [collision_flag, ~] = obj.checkCollisions(verts_near{i}, new_config);
      if ~collision_flag
      new_cost = new_config.cost + norm(new_config.coords - verts_near{i}.coords); % updated cost to get to vert
      if new_cost < verts_near{i}.cost
      % rewire!
      index_rewire = verts_near{i}.index;
      obj.verts{index_rewire}.parent = new_config.index;
      obj.verts{index_rewire}.cost = new_cost;
      end
      end
      end
      end
      end
      end
      Markdown is supported
      0% or
      You are about to add 0 people to the discussion. Proceed with caution.
      Finish editing this message first!
      Please register or to comment