First, you are asked to write several subroutines that will all be used by the RRT algorithm.

1. (10 pts) Write a function of the form given below that generates a goal-biased sample qSample

in the region defined by xmin, xmax, ymin, ymax. The function should return a random sample

with probability 1-goalBiasProb and return the goal qSample = qGoal with probability

goalBiasProb. Submit a printout of your MATLAB code.

% Usage: [qSample] = goalBiasedSample(xmin, xmax, ymin, ymax, qGoal, goalBiasProb)

%

% Description: Generates a goal- based random (x,y) configuration in the configuration space

% defined by (xmin, xmax, ymin, ymax)

%

% Inputs xmin : the minimum x-value defining the C-space

% xmax : the maximum x-value defining the C-space

% ymin : the minimum y-value defining the C-space

% ymax : the maximum y-value defining the C-space

% qGoal : the [xgoal; ygoal] column vector defining the goal location

% goalBiasProb : a value within the range, 0 <= goalBiasProb <= 1

% defining the probability with which qSample is

% returned as qSample = qGoal

%

% Output qSample : a sample in the C-space

2. (10 pts) Write a function of the form given below that returns the configuration (vertex) qNear in

the graph G that is nearest (in terms of Euclidian distance) to the sample qSample. The function

should also return the index of the vertex vid corresponding to qNear. Submit a printout of your

MATLAB code.

% Usage: [qNear, vid] = nearastSample(G, qSample)

%

% Description: Searches the graph G for the configuration qNear and corresponding vertex index (vid) that %

% Inputs G : the graph containing vertices G.V

% qSample : the sample 2D (column vector)

%

% Output qNear : the sample 2D (column vector) contained in G

% that is closest to qSample

% vid : the index of the vertex (i.e., the vertex number)

% corresponding to qNear so that qNear = G.V{vid}

3. (10 pts) Write a function of the form given below that returns a new configuration qNew that is a

distance step away from qNear in the direction of qSample. Submit a printout of your MATLAB

code.

% Usage: [qNew] = extendTree(qNear, qSample, step)

%

% Description: Generates a configuration qNew that is located a distance ’step’ away

% from qNear and along the

% directed line segment from qNear to qSample

%

% Inputs qNear : the configuration in the tree G that is closest to qSample

% qSample : a sampled configuration towards which the tree G expands

%

% Output qNew : a new sample that is located a distance ’step’ away

% from q Near in the direction of q Sample