Matlab robotics

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