CSC 576 Introduction To Robotics, Fall 2017 1

Homework 4: Probabilistic Roadmap (Due 08-Nov-2017, start of class)

1. Modify the main.m file provided and implement a 2D probablistic roadmap for a point robot
based on the pseudocode discussed in class. Generate the roadmap with a fixed number of
numRoadmapPts=100 vertices and only connect vertices that satisfy the ArePointsNear condition
(as opposed to connecting all vertices). The graph data structure G used by the code has only two
fields: vertices G.V and edges G.E that are accessed for the i-th vertex using the curly brackets
(i.e., G.V{i} gives the configuration (i.e., 2D position) of vertex i and G.E{i} gives the indices of
the vertices to which vertex i is connected to (i.e., the edges). The graph is initialized with the
robot start position as the first vertex and the goal as the second vertex and no edges.

% start
G.V{1} = params.robotCenter;
G.E{1} = [];
% goal
G.V{2} = params.goalCenter;
G.E{2} = [];

The i-th vertex configuration qSample can be added by defining G.V{i}=qSample and the edges
that are connect to that vertex are identified by index number in G.E{i}. For example, if the i-th
vertex is connected to both the start and the goal then we would define

G.E{i} = [1 2] % connect vertex i to start (vertex 1) and goal (vertex 2)

G.E{1} = [G.E{1} i] % append vertex i to the start vertex’s current edge list
G.E{2} = [G.E{2} i] % append vertex i to the goal vertex’s current edge list

2. Run your code three times each for three different numRoadmapPts settings. Make sure to turn
off plotting during the execution of your code. Complete the following table based on the output
(printed to the MATLAB command window). In a short paragraph, describe and explain the trend
you observed. What would happen if more trials were used?

numRoadmapPts=80 numRoadmapPts=100 numRoadmapPts=130

Time / Length Time / Length Time / Length
Trial 1
Trial 2
Trial 3
Average Time
Standard Deviation
Average Length
Standard Deviation

Grading: Total of 35 Points

To receive full credit for the assignment it is required that:
• (5 pts) A plot including the correct number of collision-free samples and code is provided.
• (10 pts) A plot including correctly constructed collision-free edges and code is provided.
• (10 pts) A plot including the shortest path solution is provided.

• (5 pts) Table 2 is completed, and a paragraph discussing the trend is included.

• (5 pts) Code is well commented and homework is presented in a clear, organized way.

1
CSC 576 Introduction To Robotics, Fall 2017 2

The following functions are available to you:

• rand() % a built-in MATLAB function that generates a random number from 0 to 1
• status = ArePointsNear(q1,q2)

% Checks if the points q1 and q2 are near to each other.

%
% Input q1 : a 2D vector giving the configuration q1 = [x y]’ of the robot
% q2 : a 2D vector giving the configuration q2 = [x y]’ of the robot
%
% Output status : a boolean flag equal to 1 if the points are ‘near’
% (i.e., their distance is less than 15 units) or
% a status of 0 if they are not near

• status = IsCollisionFree(params, q)

% Checks if the configuration specified by q = [x y]’ (a 2D column vector) is in collision with

% obstacles defined in params.obstacles.
%
% Input q : a 2D column vector q = [x y]’ specifying the robot’s configuration
% params : a structure containing the x,y coordinates of the obstacles
%
% Output status : 1 if configuration q is collision free, 0 if in collision

• status = IsSegmentCollisionFree(params,q1,q2)

% Checks if the straight line segment between q1 and q2 is in collision with the
% obstacles defined in params.
%
% Input q1 : a 2D column vector q1 = [x y]’ specifying the robot’s configuration
% at one endpoint of the line segment
% q2 : a 2D column vector q2 = [x y]’ specifying the robot’s configuration
% at the other endpoint of the line segment
% params : a structure containing the x,y coordinates of the obstacles
%
% Output status : boolian flag equal to 1 if configuration the segment is collision free
% equal to 0 otherwise (if there is a collision)

• PlotEdges( G, vid );

% Plots in the current figure window the location of the vertex/node specified by G.V{vid}
% Also plots straight line segments indicated all edges of vid given by G.E{vid}. This function
% be called everytime a new vertex and set of edges is added to the graph to show how
% the graph is growing with time.
%
% Input G : the graph data structured with vertices G.V and edges G.E
% vid : the index (vertex ID) indicating the vertex whose edges are to be plotted
%
% Output (none) the current figure window is updated with the desired plot