In this homework, you will rst implement the A* graph search algorithms. You will then
implement a roadmap based on a visibility graph. The problems will build on each other and
upon the code developed in Homework 1 and 2. Notice that the homework is relatively long,
but contains many optional parts.
General instructions
Homework report (required). Solve each problem, and then prepare a small PDF report
containing:
Instructions on how to run your code.
One or two sentences of comments for each question.
All analytical derivations if required.
Embedded gures and outputs that are representative of those generated by your code.
Analytical derivations. To include the analytical derivations in your report you can type
them in LATEX(preferred method), any equation editor or clearly write them on paper and
use a scanner (least preferred method).
Programming guidelines. Please refere to the guidelines on Blackboard under Informa-
tion/Programming Tips Tricks/Coding Practices.
Submission. Keep the report and the code all in one directory (do not use subdirectories).
Compress all the les into a single ZIP archive with name -hw.zip,
where and are your rst and last name, and is the homework
number (e.g., TronRoberto-hw1.zip). Submit the ZIP archive through Blackboard. Please
refer to the Syllabus on Blackboard for late homework policies.
Grading. Each question is worth 1 point unless otherwise noted. Questions marked as
optional are provided just to further your understanding of the subject, and not for credit.
Questions marked as provided have already a solution provided in the le accompanying
this assignment, which you can directly use unless you want to answer it by yourself. If you
submit an answer for these questions I will correct it, but it will not count toward your grade.
See the Syllabus on Blackboard for more detailed grading criteria.
Maximum possible score. The total points available for this assignment are 16.0 (15.0
from questions, plus 1.0 that you can only obtain with beauty points).
1
Hints Some hints are available for some questions, and can be found at the end of the
assignment (you are encouraged to try to solve the questions without looking at the hints
rst). If you use these hints, please state so in your report (your grading will not change
based on this information, but it is a useful feedback for me).
Use of external libraries and toolboxes All the problems can be solved using Matlab’s
standard features. You are not allowed to use functions or scripts from external libraries or
toolboxes other than those expressly provided.
Graph data structure
Both problems in this homework represent the workspace as a graph. In practical terms, the
graph will be represented by a structure array graphVector, where each element of the array
is a struct with elds:
neighbors (dim. [NNeighbors 1]): an array containing the indexes (in graphVector)
of the vertices that are adjacent to the current one.
neighborsCost (dim. [NNeighbors 1]): an array, with the same dimension as the
eld neighbors, containing the cost to move to each neighbor.
g (dim. [1 1]): scalar variable to store the cost from the starting location along the
path through the backpointer.
backpointer (dim. [1 1]): index of the previous vertex in the current path from the
starting location.
x (dim. [2 1]): the physical (x;y) coordinates of the vertex.
Note that, in the above, the dimension NNeighbors is in general di erent for each element in
graphVector. Moreover, the only elds that will be modi ed by the graph search algorithm
are g and backpointer. The other elds remain constant.
Problem 1: Graph search
In this problem you will implement a graph search algorithm, and apply it to a graph obtained
from a grid discretization of a free con guration space. In particular, you will apply this to
the two-link manipulator from Homework 3.
The graph search function you will develop will be generic, because it can work on a
graphVector data structure in a way that is somewhat abstract from the actual problem.
For instance, the function manipulates nodes in terms of their indexes in the data structure,
instead of, say, using their coordinates. In this way, the same function can be applied to
di erent problems (an occupancy graph in this problem and a roadmap in the next).
You will be required to implement the A* algorithm, for which the reference pseudo-code
for the algorithm can be found on page 531 of the book, and is reproduced in Algorithm 1
with some additional minor clari cations.
Question 1.1 ( provided ). This function should takes as input a discretized world and
outputs the corresponding graphVector structure.
[graphVector]=grid2graph(xGrid,yGrid,flagFreeSpaceMap)
Description: The function should return a graphVector structure described by the
2
inputs. See Figure 1 for an example of the expected inputs and outputs.
Input arguments
xx (dim. [NGrid 1]): array with the x coordinates of the vertical lines in the
discretization grid.
yy (dim. [NGrid 1]): array with the y coordinates of the horizontal lines in the
discretization grid.
flagFreeSpaceMap (dim. [NGrid NGrid], type logical): an array such that
flagFreeSpaceMap(i,j) is true if there is a cell (i.e., no collision) at the
(xx(i);yy(j)) location, and false otherwise.
Output arguments
graphVector (dim. [NGrid*NGrid,1 :], type struct) structure array (as dis-
cussed above), obtained from the points on the grid using a 8-neighbors connectivity.
For the cost to move from one neighbor to the other, use the Euclidean distance.
Note that, taken together, the inputs are very similar to the grid struct from Homework
3). Moreover, xx and yy are to be intended as a generalized coordinate pair, and their
interpretation could be di erent than x and y coordinates of points in R2. For instance, in
Questions 2.2 below involving the two-link manipulator, they correspond to angles.
Question 1.2 ( provided ). Make a function graph_plot(graphVector) that produces a
visualization of the content of the graphVector structure. Note that this function is inteded
to plot only relatively small graphs for debug purposes, such as the one in Figure 1.
Question 1.3 (0.5 points).
Algorithm 1 The A* algorithm.
1: Add the starting node nstart to O, and set g(nstart) = 0. . Initialization
2: repeat
3: Pick nbest from O such that f(nbest) f(n) for all n2O.
4: Remove nbest from O and add it to C.
5: if nbest = qgoal then
6: Exit.
7: end if
8: for all x2Star(nbest) that are not in C do . Expand nbest
9: if x =2O then
10: Set the value of g(x) to g(nbest) +c(nbest;x).
11: Set the backpointer of x to nbest.
12: Add x to O with value f(x).
13: else if g(nbest) +c(nbest;x) 14: Update the value of g(x) to g(nbest) +c(nbest;x).
15: Update the backpointer of x to nbest.
16: end if
17: end for
18: until O is empty
(b) Graphical representation
of the graph with 8-neighbors
connectivity. The num-
bers represent the indexes
in graphVector. The name
flagFreeSpaceMap has been
abbreviated to flag for con-
venience.
neighbors=[4;2]
neighborsCost=[1;1]
x=[1;1]
neighbors=[1;4;3]
neighborsCost=[1;1.414;1]
x=[1;2]
neighbors=[2]
neighborsCost=[1]
x=[1;3]
neighbors=[1;2]
neighborsCost=[1;1.414]
x=[2;1]
(c) The output struct array graphVector
(the elds g and backpointer are not visu-
alized). This particular graphVector can
be found in the le graph_testData.mat.
Figure 1: Example of the input and output arguments for grid2graph(). Update: the
ordering of the nodes have been modi ed to re ect the actual results of the function provided.
[h]=graph_heuristic(graphVector,idxX,idxGoal)
Description: Computes the heuristic h given by the Euclidean distance between the
nodes with indexes idxX and idxGoal.
Input arguments
graphVector (dim. [NNodes 1], type struct): the structure describing the
graph, as speci ed above.
idxX (dim. [1 1]), idxGoal (dim. [1 1]): indexes of the elements in
graphVector to use to compute the heuristic.
Output arguments
h (dim. [1 1]): the heuristic (Euclidean distance) between the two elements.
Question 1.4 (0.5 points).
[idxExpand]=graph_getExpandList(graphVector,idxNBest,idxClosed)
Description: Finds the neighbors of element idxNBest that are not in idxClosed (line 8
in Algorithm 1).
Input arguments
graphVector (dim. [NNodes 1], type struct): the structure describing the
4
graph, as speci ed above.
idxNBest (dim. [1 1]): the index of the elment in graphVector of which we
want to nd the neighbors.
idxClosed (dim. [NClosed 1]): array of indexes containing the list of elements
of graphVectors that have been closed (already expanded) during the search.
Output arguments
idxExpand (dim. [NNeighborsNotClosed 1]): array of indexes of the neighbors
of graphVector that are not in idxClosed.
Question 1.5.
[graphVector,pqOpen]=graph_expandElement(graphVector,idxNBest,idxX,idxGoal,pqOpen)
Description: This function expands the vertex with index idxX (which is a neighbor of
the one with index idxNBest) and returns the updated versions of graphVertex and
pqOpen.
Input arguments
graphVector (dim. [NNodes 1], type struct): the structure describing the
graph, as speci ed above.
idxNBest (dim. [1 1]), idxX (dim. [1 1]), idxGoal (dim. [1 1]): indexes
in graphVector of the vertex that has been popped from the queue, its neighbor
under consideration, and the goal location.
This function corresponds to lines 9{16 in Algorithm 1.
Question 1.6. Implement a function that transforms the backpointers describing a path
into the actual sequence of coordinates.
[xPath]=graph_path(graphVector,idxStart,idxEnd)
Description: This function follows the backpointers from the node with index idxEnd in
graphVector to the one with index idxStart node, and returns the coordinates (not
indexes) of the sequence of traversed elements.
Input arguments
idxStart (dim. [1 1]), idxEnd (dim. [1 1]): indexes in graphVector of the
starting and end vertices.
Output arguments
xPath (dim. [2 NPath]): array where each column contains the coordinates of
the points obtained with the traversal of the backpointers (in reverse order). Note
that, by de nition, we should have xPath(:,1)=graphVector(idxStart).x, and
xPath(:,end)=graphVector(idxEnd).x.
Question 1.7. This question puts together the answers to Questions 1.3{1.6.
[xPath]=graph_planner(graphVector,idxStart,idxGoal)
Description: Implements the A* algorithm, as described by the pseudo-code in Algo-
rithm 1.
5
Input arguments
graphVector (dim. [NNodes 1], type struct): the structure describing the
graph, as speci ed above.
idxStart (dim. [1 1]), idxEnd (dim. [1 1]): indexes in graphVector of the
starting and end vertices.
Output arguments
xPath (dim. [2 NPath]): array where each column contains the coordinates of
the points of the path found from idxStart to idxEnd.
For the purposes of this homework, you can assume that a path always exists (although this
can be optionally relaxed in Question 1.8).
The function for the cost to use in the priority queue, denoted as f(n) in the book and
in Algorithm 1, is f(n) = g(n) + h(n), where g(n) is the cost of the path from node n to
the start vertex (going through the backpointer), and h(n) is the heuristic (the Euclidean
distance between nodes, see below). The cost c(n;x) between two vertices is the one stored
in the neighborsCost eld.
Your implementation of graph_planner() must contain the following elements:
a priority queue pqOpen of the opened vertices (the structure O in Algorithm 1); this
structure must be manipulated with the functions priority*() from Homework 1; use
the index of the vertex for the key and the function f(n) described above for the cost.
The supporting les for this homework include the function priorityIsMember(), which
you can use to check if an element with a given key is in the queue or not.
a vector idxClosed (dim. [NClosed 1]) containing the indexes of the closed vertices.
Question 1.8 ( optional ). Add conditions to return an empty path if A* cannot nd a
feasible path.
Question 1.9 ( optional ). Add an argument method containing a string that determines
the behavior. of the algorithm. The function f(n) will then depend on the value of the
argument method:
f(n) = g(n) if method is equal to bfs.
f(n) = h(n) if method is equal to greedy.
f(n) = g(n) +h(n) if method is equal to astar.
Question 1.10 ( optional ). Make a function graph_planner_test() that uses grid2graph()
to build some small arbitrary graph, calls graph_planner() to nd a path between two nodes,
and then plots the results. Try to use di erent arbitrary graphs, and visually inspect if the
results make sense. This will give you the con dence that your A* algorithm works well.
Since this routine is the backbone of the next questions, I strongly encourage you to make
sure that it works properly. As a starting point, you could use the very simple graphVector
detailed in Figure 1, which is also given in the supporting le graph_testData.mat.
Problem 2: Application of A* to the two-link manipulator
In this problem you will apply the graph search function you implemented in Problem 1 to the
two-link manipulator from Homework 2. In this case, the coordinates in graphVector().x
6
will represent the pairs of angles ( 1; 2) for the two links (as was speci ed in Homework 2).
Question 2.1 (0.5 points). The le twolink_freeSpaceData.mat contains the variables
xxTheta, yyTheta and flagFreeSpaceMap that describes the con gurations of angles for the
two-link manipulator that collide with the set of points in twolink_testData.mat. These
variables are essentially the result of the optional Question 3.6 from Homework 2 (please
reread that question for details). For this question, you need to implement a function
twolink_freeSpaceGraph() that
1) Loads the contents of twolink_freeSpaceData.mat.
2) Calls grid2graph(.)
3) Stores the resulting vectorGraph struct array in the le twolink_freeSpaceGraph.mat
together with the variables xxTheta and yyTheta.
Question 2.2.
[thetaPath]=twolink_search(thetaStart,thetaEnd)
Description: This function performs the following operations:
1) Loads graphVector from twolink_freeSpaceGraph.mat.
2) Identi es the two indexes idxStart, idxEnd in graphVector that correspond
to locations on the grid de ned by xxTheta and yyTheta that are closest to
thetaStart and thetaEnd.
3) Calls graph_planner() to nd a feasible sequence of angles thetaPath from
idxStart to idxEnd.
4) Appends thetaStart and thetaEnd, respectively, to the beginning and the end
of the array thetaPath.
Input arguments
thetaStart (dim. [2 1]), thetaEnd (dim. [2 1]): vectors of angles