Description
Problem 1:
Solve the following planning problem using an RRT. For a point robot in the 2D space pictured
below find a path from qs = (4.5, 6.5) (green circle) to qg = (10.5, 0.5) (pink circle) while avoiding
the obstacles (red squares). The obstacles are two units on each side and have centers at (6,5),
(8.5,1.5), and (10.5,2). If you want, you may use a bidirectional RRT that builds two trees, one
rooted at the start point and one rooted at the goal point. Stop expanding your tree when the
Euclidean distance between the goal and a point on the tree is less than 0.1.
(0,0)
(12,10)
qs
= (4.5,6.5)
qg
= (10.5,0.5)
(a) Plot the tree. Have you code make a plot of the space including the obstacles. Plot the
start point as a green circle and the goal point as a magenta (color ‘m’ in Matlab) circle. Plot all
edges on the tree as blue lines. Plot the path connecting the start and goal points as green lines.
(b) Report vertices. Report the number of vertices in your tree on the command line using
display([’vertices =’,num2str(vertices)]).
(c) Report edges. Report the number of edges in the path connecting the start and goal points
on the command line using display([’edges =’,num2str(edges)]).
