Graph Search Algorithms In Matlab

studentproj2/maps/map1.txt # map3 block 12.000000 0.000000 12.900000 3.000000 255.000000 0.000000 0.000000 block 6.000000 2.000000 6.900000 5.000000 0.000000 0.000000 255.000000 boundary 0.000000 0.000000 20.000000 5.000000 __MACOSX/studentproj2/maps/._map1.txt studentproj2/maps/map2.txt # map2 block 4.000000 1.500000 5.000000 5.000000 255.000000 0.000000 0.000000 block 7.500000 0.000000 8.500000 3.000000 255.000000 0.000000 0.000000 block 10.500000 1.5000000 11.50 5.0 0.0 0.0 255.0000 boundary 0.000000 0.000000 20.000000 5.000000 __MACOSX/studentproj2/maps/._map2.txt studentproj2/maps/map3.txt # map2 boundary 0.000000 0.000000 20.000000 5.000000 __MACOSX/studentproj2/maps/._map3.txt __MACOSX/studentproj2/._maps studentproj2/runsim.m %% Instructor's Note: This is where you run the code. close all; clear all; clc; addpath(genpath('./')); %% Plan path disp('Planning ...'); %% Loading map %% Instructor's Note: If you want to test out your shortestpath algorithm without blocks, use map3.txt. map = load_map('maps/map3.txt', 0.1, 0.25); % xy_res, z_res, margin start = [1.0 3.0]; stop = [19.0 3.0]; %% load the plot %false for dijkstra, true for Astar [path,distance_min] = shortestpath(map, start, stop, false); %% Phase selection %only change phase1!! If you are working on Phase 1 set it true, if you are %working on phase 2 then set phase 1 = false phase1 = true; %if you are plotting shortest path (either dijkstra or Astar) phase2 = ~phase1; %if you are plotting the trajectory generation %% Part 1: if phase1 plot_path(map, path); %% Part 2: elseif phase2 %Additional init script init_script; %%Run trajectory trajectory = test_trajectory(start, stop, map, path, true); % with visualization end __MACOSX/studentproj2/._runsim.m studentproj2/shortestpath.m function [path, distance] = shortestpath(map, start, goal, astar) % DIJKSTRA Find the shortest path from start to goal. % [PATH, DISTANCE] = shortestpath(map, start, goal) returns an M-by-2 matrix, where each row % consists of the (x, y) coordinates of a point on the path. The first % row is start and the last row is goal. If no path is found, PATH is a % 0-by-2 matrix. % % [PATH, DISTANCE] = shortestpath(map, start, goal, astar) finds the path using euclidean % distance to goal as a heuristic if astar is true. % % DISTANCE - the distance of the path returned by the shortest path % algorithm % if nargin < 4 astar = false; end %% %%%%%%%%initialization%%%%%%%% nodenumber = map.nodenumber; leftbound = map.boundary(1,1:2); blockflag = map.blockflag; resolution = map.resolution; xy_res = resolution(1); m = map.segment; mx = m(1); my = m(2); [row,~] = size(nodenumber); cstart = worldtocell(leftbound,resolution,start); nodestart = celltonumber(m,cstart); cgoal = worldtocell(leftbound,resolution,goal); nodegoal = celltonumber(m,cgoal); %please initialize the rest of the requirements here %% %A* algorithm and dijkstra algorithm if astar %% % Code here for A* algorithm else %% %%Code here for dijkstra algorithm end path = []; distance = 0; __MACOSX/studentproj2/._shortestpath.m studentproj2/traj_time.m function [desired_state] = traj_time(t,coefficient, timepoint,stop) %Produces the desired state based on the coefficient matrx (output of %traj_coefficient, t, and stop % Uses the timepoint array, stop, and t to compute a desired state based on the % coefficient matrix % stop is the goal positions (declared in runsim) % timepoint is the time at every point of the path end __MACOSX/studentproj2/._traj_time.m studentproj2/map/worldtocell.m %% Instructor's Note: Please do not touch. function A = worldtocell(leftbound,resolution,point) %lowleftbound = map.boundary(1,1:3); %resolution = map.resolution; A = fix((point - leftbound)./resolution)+1; end __MACOSX/studentproj2/map/._worldtocell.m studentproj2/map/celltoworld.m %% Instructor's Note: Please do not touch. function A = celltoworld(leftbound,resolution,point) % lowleftbound = map.boundary(1,1:3); % resolution = map.resolution; nx = point(1); ny = point(2); xy_res = resolution(1); A(1) = xy_res/2 + (nx-1)*xy_res+leftbound(1); A(2) = xy_res/2 + (ny-1)*xy_res+leftbound(2); end __MACOSX/studentproj2/map/._celltoworld.m studentproj2/map/load_map.m %% Instructor's Note: Please do not touch. function map = load_map(filename, xy_res, margin) % LOAD_MAP Load a map from disk. % MAP = LOAD_MAP(filename, xy_res, z_res, margin). Creates an occupancy grid % map where a node is considered fill if it lies within 'margin' distance of % on abstacle. fid = fopen(filename); data = textscan(fid,'%s %f %f %f %f %f %f %f','CommentStyle','#'); [rowdata coldata] = size(data{1,1}); rawdata = [data{1,2} data{1,3} data{1,4} data{1,5} data{1,6} data{1,7} data{1,8}]; blockline = 1; [n m] = ismember('boundary',data{1,1}); for counterdata = 1:rowdata if counterdata == m basicdata(1,:) = rawdata(counterdata,:); elseif counterdata ~= m blockline = blockline + 1; basicdata(blockline,:) = rawdata(counterdata,:); end end [row column] = size(basicdata); %the low left corner of the boundary and %the upper right corner of the boudary leftcorner_bound = basicdata(1,1:2); rightcorner_bound = basicdata(1,3:4); %the low left corner of the blocks and %the upper right corner of the blocks if (row >= 2) leftcorner_block = basicdata(2:row,1:2) - margin; rightcorner_block = basicdata(2:row,3:4) + margin; %expand the matrix of the left corner of the boundary into %(row-1)*3 matrix. So is the resolution matrix for counter1 = 2 : row leftcorner_blockstart(counter1-1,:) = leftcorner_bound(1,1:2); resolution(counter1-1,:) = [xy_res xy_res]; end %divide x dimension into mx parts %divide y dimension into my parts m = ceil((rightcorner_bound - leftcorner_bound)./[xy_res xy_res]); mx = m(1); my = m(2); %Here the each node of the map is given a number following rules: %1. increasing from left to right. node(:,1) = 1 : (mx + (my-1)*mx); flag(:,1) = zeros(mx + (my-1)*mx,1); %calculate which node the rightcorner and leftcorner of the blocks %lied in. n_upright_block = fix((rightcorner_block - leftcorner_blockstart)./resolution)+1; n_lowleft_block = fix((leftcorner_block - leftcorner_blockstart)./resolution)+1; [row1 column1] = size(n_upright_block); for i = 1:row1 if n_upright_block(i,1) > mx n_upright_block(i,1) = mx; end if n_upright_block(i,2) > my n_upright_block(i,2) = my; end for j = 1:column1 if n_lowleft_block(i,j) <= 0 n_lowleft_block(i,j) = 1; end end end %All the nodes that the blocks lie in is given a flag as 1 counter4 =1; for i = 1:row-1 for counter2 = n_lowleft_block(i,1) : 1 : n_upright_block(i,1) for counter3 = n_lowleft_block(i,2) : 1 : n_upright_block(i,2) nodeposition = counter2 + (counter3 - 1)*mx+(counter4-1)*mx*my; flag(nodeposition,1) = 1; end end end map.nodenumber = node; map.blockflag = flag; map.margin = margin; map.segment = m; map.boundary = basicdata(1,1:4); map.block(1:row-1,1:2) = n_lowleft_block; map.block(1:row-1,3:4) = n_upright_block; map.block(1:row-1,5:7) = basicdata(2:row,5:7); map.obstacle_vertices = zeros(2,4,row-1); for i = 1:row-1 map.obstacle_vertices(:,1,i) = leftcorner_block(i,:)'; map.obstacle_vertices(:,2,i) = [leftcorner_block(i,1),rightcorner_block(i,2)]'; map.obstacle_vertices(:,3,i) = [rightcorner_block(i,1),leftcorner_block(i,2)]'; map.obstacle_vertices(:,4,i) = rightcorner_block(i,:)'; end map.resolution = [xy_res xy_res]; map.basicdata = basicdata; else %divide x dimension into mx parts %divide y dimension into my parts m = ceil((rightcorner_bound - leftcorner_bound)./[xy_res xy_res]); mx = m(1); my = m(2); %Here the each node of the map is given a number following rules: %1. increasing from left to right. node(:,1) = 1 : (mx + (my-1)*mx); flag(:,1) = zeros(mx + (my-1)*mx,1); map.nodenumber = node; map.blockflag = flag; map.margin = margin; map.segment = m; map.boundary = basicdata(1,1:6); map.block = []; map.resolution = [xy_res xy_res]; map.basicdata = basicdata; end end __MACOSX/studentproj2/map/._load_map.m studentproj2/map/numbertocell.m %% Instructor's Note: Please do not touch. function cellcoordinate = numbertocell(map,number) m = map.segment ; mx = m(1); my = m(2); % nz = ceil(number/(mx*my)); ny = ceil((number)/mx); % ny = ceil((number - (nz-1)*mx*my)/mx); nx = number - (ny-1)*mx; % nx = number - (ny-1)*mx - (nz-1)*mx*my; cellcoordinate = [nx ny]; % cellcoordinate = [nx ny nz]; end __MACOSX/studentproj2/map/._numbertocell.m studentproj2/map/celltonumber.m %% Instructor's Note: Please do not touch. function A = celltonumber(segment,point) mx = segment(1); my = segment(2); A = point(1) + (point(2)-1)*mx; % A = point(1) + (point(2)-1)*mx + (point(3)-1)*mx*my; end __MACOSX/studentproj2/map/._celltonumber.m __MACOSX/studentproj2/._map studentproj2/init_script.m %% Instructor's Note: Please do not touch. % Generate trajectory disp('Generating Trajectory ...'); %trajectory_generator([], map, path); __MACOSX/studentproj2/._init_script.m studentproj2/test_trajectory.m %% Instructor's Note: Please do not touch. function [x] = test_trajectory(start, stop, map, path, vis) % TEST_TRAJECTORY simulates the robot from START to STOP following a PATH % that's been planned for MAP. % start - a 2d vector % stop - a 2d vector % map - map generated by your load_map % path - n x 2 matrix path planned by your shortest path algorithm % vis - true for displaying visualization % trajhandle = @trajectory_generator; %% **************************** FIGURES ***************************** % Environment figure if nargin < 5 vis = true; end fprintf('Initializing figures...\n') if vis h_fig = figure('Name', 'Environment'); else h_fig = figure('Name', 'Environment', 'Visible', 'Off'); end plot_path(map, path); h_3d = gca; drawnow; xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]') %quadcolors = lines(nquad); set(gcf,'Renderer','OpenGL') %% *********************** INITIAL CONDITIONS *********************** fprintf('Setting initial conditions...\n') % Maximum time that the robot is allowed to move about space time_tol = 13; % maximum simulation time starttime = 0; % start of simulation in seconds tstep = 0.05; % this determines the time step at which the solution is given cstep = 0.25; % image capture time interval nstep = cstep/tstep; time = starttime; % current time max_iter = time_tol / cstep; % max iteration x0 = start; xtraj = zeros(max_iter, length(x0)); [coefficient,timepoint] = traj_coefficient(map, path); %% ************************* RUN SIMULATION ************************* curve = animatedline('LineWidth', 2); fprintf('Simulation Running....\n') for iter = 1:max_iter timeint = time:tstep:time+cstep; tic; if iter == 1 plot_path(map, path) hold on h_title = title(sprintf('iteration: %d, time: %4.2f', iter, time)); end time_now = tstep*(iter-1)*5; %desired_state = trajhandle(time_now); desired_state = traj_time(time_now,coefficient, timepoint,stop); pos = desired_state.pos; posx = pos(1); posy = pos(2); hold on addpoints(curve, posx, posy); head = scatter(posx, posy, 'rX'); %pause(0.01) % drawnow xtraj(iter,:) = pos'; set(h_title, 'String', sprintf('iteration: %d, time: %4.2f', iter, time + cstep)) time = time + cstep; % Update simulation time t = toc; %fprintf('the time is %d \n',t) % Pause to make real-time if (t < cstep) pause(cstep - t); end end fprintf('Simulation Finished....\n') x=xtraj; end __MACOSX/studentproj2/._test_trajectory.m studentproj2/handout.pdf ROB-3303 Project 2 Due: Thursday, December 9, 2021, 11:59 pm 1 Overview In this assignment you will begin building a trajectory generator for a point mass robot by implement two graph search algorithms: dijkstra’s algorithm and a popular variant called A*. The goal of this phase is to compute resolution optimal paths in a 2D environment from a start position to a goal position that avoid colliding with obstacles. The environments used for this assignment are defined in text files and example of one such file is shown here: # An example environment # boundary xmin ymin xmax ymax # block xmin ymin xmax ymax r g b boundary 0.0 0.0 20.0 5.0 block 3.10 0.0 3.90 5.0 255.0 0.0 0.0 block 9.10 0.0 9.90 5.0 255.0 0.0 0.0 where # is used for comments and the boundary of the environment as well as block obstacles are defined in terms of their minimum (lower-left-near) and maximum (upper-right-far) corners stored in a row vector as [x_min y_min x_max y_max]. The block definition also includes a triplet of RGB color values used in the visualization. In order to find paths with a graph search algorithm it is necessary to first represent the environment as a graph. To do this we discritize space into a regular grid and attach graph nodes to each voxel. The choice of grid resolution is especially important and impacts both the runtime of the algorithm as well as its ability to find a path. Another consideration is safety, which becomes critical when considering real world applications. We consider for simplicity that our robot is an infinitesimal point. Properly formatted maps may be loaded using the provided load_map function which takes the name of the textfile, grid resolution, and obstacle margin as inputs and returns a struct with map properties and a 2D matrix representing the voxel grid. Voxels with value 0 enclose free space while voxels with value 1 intersect obstacles. The function inflates each obstacle by a distance of margin in each direction before discritizing and marking the intercepted voxels as occupied. For indexing purposes, the origin of each voxel is considered to be the lower-left-front corner (the minimum corner of the voxel’s bounding box). Another function called plot_path can be used for visualizing maps and paths. More detail can be found in the header descriptions of both files. NOTE: a number of helper functions for indexing and visualizing 2D maps discritized into voxel grids are described in Section 4. 2 Graph Search 2.1 Dijkstra Your main task is to implement Dijkstra’s algorithm as described in class. A thorough description of the inputs and expected outputs can be found in the header of the shortestpath.m included in the project packet. See the description in the header of shortestpath. You are also not allowed to use any external libraries or implementations on this assignment. 1 2.2 A-Star A popular variant of Dijkstra’s algorithm is the A* algorithm. Since both algorithms are very similar, modify your completed shortestpath function, it uses distance to the goal as a heuristic to guide the search. Remember that any heuristic must be admissible and valid; if not, the algorithm is no longer guaranteed to return the shortest path. 3 Trajectory Generation Unfortunately, the optimal path output from your implementation of Dijkstra or A* usually is not smooth due to the voxel grid based discretization of the environment. To improve the performance, you may apply trajectory smoothing techniques to convert the path into smooth trajectories that the robot can track. One example will be polynomial segments as discussed in class. You will have to determine the start and end points of the polynomial curve as well as all other boundary conditions. Complete the implementation of trajectory_generator.m. This function takes a path from Dijkstra or A* and converts it into a trajectory as a function of time. You are allowed to use the map for trajectory generation. The overall trajectory needs to be executed in 10 s. You can consider that your points time allocation is equally and you want guarantee at each point the velocity to be continuous and a given value that you choose. The initial and final velocities at the start and goal positions should be set to 0. Your trajectory may deviate from the straight line path connecting the points generated by your graph search algorithm. Therefore, you should set your speed carefully. You should make sure that no part of the robot collides with any obstacles. 4 Helper Functions The following helper function aid in indexing and visualizing 2D maps stored as voxel grids • map = load_map(filename, xy_res, margin): creates an occupancy grid map where a node is considered fill if it lies within ’margin’ distance of on obstacle. • A = celltoworld(leftbound,resolution,point): converts the cell metric coordinates to the corresponding cell coordinates. • A = worldtocell(leftbound,resolution,point): converts the cell coordinates to the corre- sponding cell metric coordinates. • cellcoordinate = numbertocell(map,number): converts a cell index to the cell world coor- dinates. • A = celltonumber(segment,point): convert the cell world coordinates to the cell index. 5 Report You need to summarize your results in a report submitted in pdf format and generated with latex or word. Please add on top of your manuscript your name and NYU ID. The report should not be more than 8 pages including plots. You will have to use the plot function that has been provided in the code to generate your results for the 2 given datasets. In addition to the results, please include your approach and any explanation you think is appropriate. Try to add your logic process and explain why. Moreover, briefly comment your plots and compare your results to the ground truth. The plot function already overlaps your filter estimates with the ground truth. 2 6 Grade Policy and Submission The overall score will be 100 and will be subdivided in the following way, Dijkstra (40 points), A-Star (20 points), trajectory generation (30 points), and report quality and readability (10 points). Do not modify any part of the code except the required functions. All the files, including code and report, should be submitted in an unique zip file. Please write your full name, email, and NYU netID on top of the report. The report should be written in word or latex and submitted as a pdf file 3 Overview Graph Search Dijkstra A-Star Trajectory Generation Helper Functions Report Grade Policy and Submission __MACOSX/studentproj2/._handout.pdf studentproj2/plot_path.m %% Instructor Note: Please do not touch function plot_path(map, path, coefficient, timelist) % PLOT_PATH Visualize a path through an environment % PLOT_PATH(map, path) creates a figure showing a path through the % environment. path is an N-by-3 matrix where each row corresponds to the % (x, y, z) coordinates of one point along the path. configuration = map.basicdata; [len ~] = size(configuration); hold on; if nargin == 2 for i = 1 : len center_x = (configuration(i,1) + configuration(i,3))/2; center_y = (configuration(i,2) + configuration(i,4))/2; x_res = configuration(i,3) - configuration(i,1); y_res = configuration(i,4) - configuration(i,2); x_down = center_x - x_res/2; x_up = center_x + x_res/2; y_down = center_y - y_res/2; y_up = center_y + y_res/2; X = [x_down x_up x_up x_down]; Y = [y_down y_down y_up y_up]; if i == 1 Color = 'w'; alpha = 0.1; else Color = 'k'; alpha = 0.5; end fill(X,Y,Color,'FaceAlpha',alpha) end if ~isempty(path) plot(path(:,1),path(:,2),'b-') hold off; end xlabel('x') ylabel('y') elseif nargin == 4 end end __MACOSX/studentproj2/._plot_path.m studentproj2/traj_coefficient.m function [coefficient,timepoint] = traj_coefficient(map, path) %Produces the coefficient matrix and timepoint array % Given a path and a map, the coefficient matrix is iteratively found % based on the constraints and constraint conditions % map: The map structure returned by your load_map function % path: This is the path returned by your planner (dijkstra function) end __MACOSX/studentproj2/._traj_coefficient.m __MACOSX/._studentproj2