Note: this lab does not inviolve GoPiGo programming. You may program the problem below in whatever language you like, using any graphics you like. Just make sure you include any resources needed to execute this code with your submission.

References: Rapidly-exploring Random Trees by S. LaValle and J. Kuffner

Part I: (80 points. Write a progam to find a path in a 2-D planar environment with obstacles using an RRT algorithm. Your RRT planner should use a single directional search from the start point towards the goal point. You may assume a point based robot - no rotation is needed. You should also have a command line parameter that sets the DISTANCE that you will grow your tree at each iteration. You can access a file describing the obstacles and a file describing the start and goal coordinates. The obstacles are contained within a 600x600 environment. Format of the obstacles file is as follows

• first integer gives you the number of obstacles
• for each obstacle:
  • first integer gives you the number of vertices
  • the vertices follow as X Y pairs, one per line, each with two coordinates
  • You should close the obstacle by linking the last vertex to the first

The text file containing the start and goal point has the format where the first line (X Y coordinates) determines the start point and second line (X Y coordinates) determines the goal point

Below is a picture of the obstacle course.

Your program should display:

• The obstacle set
• the random tree that is grown.
• the shortest path between start and goal using the tree.

The image below shows a solution (Note: every solution is different since this is a stochastic algorithm):

Part II: (20 points) Make your RRT bi-directional - have one tree grow from the start point and another tree grow from the goal point, merging the two trees when they meet. Display the same information as in part I above but use a different color for each of the two trees as they are grown.

Extra Credit (up to 10 points): Translation and Rotation. Assume a rectangular robot with coordinates (0,0), (20,0), (20,50) and (0,50), and we will call this configuration zero degrees (i.e. robot is vertical or upright). The reference point on the robot is its centroid (10,25), and the robot can rotate about this point. Find an RRT collision free path for this robot using the obstacle set above where the reference point starts at location (75,50) in zero degree configuration and the goal is the reference point at location (482,577) with the robot rotated +90 degrees about its reference point (robot is horizontal). Display the same information as in the point robot above, but also display the robot's orientation as it moves along the safe path. Animations are especially nice to provide!