$35
Project: Rapidly-Exploring Random Trees Solution
1 Background
Path Planning is a problem in Robotics that involves generating paths from a start to a goal point that do not collide with obstacles. In this project, you will implement an algorithm called Rapidly-Exploring Random Tree (RRT) to plan for collision-free paths in 2-dimensional space. The figure below shows an example result: the red dot indicates the start location, the green dot the goal location, the blue lines the built tree, and the green lines the obstacle-free path found.
2 Provided Geometry Classes and Functions
We have provided the following geometry helper functions and classes that you will need for this project, in the geometry object:
The Point class is used to represent a two dimensional point, with member variables x and y:
let p = new geometry.Point(12, 34);
console.log(p.x);
console.log(p.y);
Output:
12
34
The Line class is used to represent a two dimensional line segment, with member variables p1 and p2 to represent the end-points of the line-segment:
let l = new geometry.Line(new geometry.Point(1, 2), new geometry.Point(3, 4));
console.log(l.p1);
console.log(l.p2);
Output:
{
1,
2
}
{
3,
4
}
The intersects function is used to compute whether or not two line-segments intersect:
let l1 = new geometry.Line(new geometry.Point(-1, 0), new geometry.Point(1, 0)); let l2 = new geometry.Line(new geometry.Point(0, -1), new geometry.Point(0, 1)); console.log(geometry.intersects(l1, l2));
let l3 = new geometry.Line(new geometry.Point(-1, 0), new geometry.Point(1, 0)); let l4 = new geometry.Line(new geometry.Point(0, 1), new geometry.Point(0, 2)); console.log(geometry.intersects(l3, l4));
Output:
true
false
3 Programming Task
Overview
You will implement a Tree class, and four functions (distance, buildTree, getPath, samplePoint) that will be used to implement the RRT algorithm. The RRT algorithm itself will be implemented in a function called plan(start, goal, map, options). You may add any additional classes or functions you feel are necessary.
Required Helper Classes
Define a Tree class that has the following member variables:
node: The Point that represents the 2D location of the tree's current node
children: An array of instances of Trees, which correspond to the children of the tree’s current node.
parent: Reference to the instance of the Tree class that is the parent of the current node.
The Tree class will have the following methods:
nearest(p), that takes an instance p of the Point class, and returns the root of a subtree that has the node located closest to the point p. Note that this method must return a reference to a Tree object, not a Point object.
extend(p, maxExtension), that takes an instance p of the Point class, and returns a point that extends from the current Tree’s node towards p, up to a maximum distance maxExtension. If the point p is closer than maxExtension, it returns p.
add(p), that takes an instance p of the Point class, and adds a new child leaf at the location p to the tree.
Required Helper Functions
Define the following functions:
distance(p1, p2) that takes two points, and returns the distance between the two points given by the formula √(p1. x − p2. x)2 + (p1. y − p2. y)2
samplePoint(mapSize, goal, goalBias) that takes the size of the map, the goal point, and the goal bias, and returns a randomly sampled 2D point within the map. A certain fraction of the time (specified by goalBias), this function will return the goal point itself.
collides(map, p1, p2) that takes in a map as an array of line segments, and returns true if the line segment joining p1 and p2 intersects with any of the line segments in the map, and false otherwise.
getPath(leaf, goal) that takes in a Tree leaf that is closest to the goal, and returns the full path found, as an array of points. The first point in the array must be the goal, and the last point the start point.
The Map
The map in which the RRT planner will work on is defined by two things: its size and the obstacles inside of it. All the maps in this project will have a square shape: their height and width will be equal. Thus, the size of a map can be represented by a single numerical variable, named mapSize which will reside in the options object (see the following section).
The walls, or obstacles, of the map are represented by a single array containing several Line objects. Each of this objects corresponds to exactly one wall inside the map. Consider the following map, which is the same as the one in the first figure:
This map can be represented with the following code:
let emptyCallback = function(p, q, r, t) {}; let options = {
mapSize: 400,
maxExtension: 10,
goalBias: 0.05,
maxSamples: 10000,
callback: emptyCallback
};
let map = [
new geometry.Line(new geometry.Point(0, 0), new geometry.Point(0, 400)),
new geometry.Line(new geometry.Point(400, 400), new geometry.Point(0, 400)),
new geometry.Line(new geometry.Point(400, 400), new geometry.Point(400, 0)),
new geometry.Line(new geometry.Point(400, 0), new geometry.Point(0, 0)), new geometry.Line(new geometry.Point(0, 200),
new geometry.Point(300, 200)),
new geometry.Line(new geometry.Point(0, 100), new geometry.Point(200, 100)),
new geometry.Line(new geometry.Point(300, 100), new geometry.Point(400, 100)), new geometry.Line(new geometry.Point(100, 200), new geometry.Point(100, 300)),
new geometry.Line(new geometry.Point(200, 300), new geometry.Point(200, 400)), new geometry.Line(new geometry.Point(300, 300), new geometry.Point(400, 300)),
];
Putting Everything Together: The RRT Planner
Define the function plan(start, goal, map, options) ,to implement the RRT algorithm to find a collision-free path from the start location to the goal location (provided as instances of the Point class). The options object provides options for the algorithm, and has the following members:
mapSize: The size of the map, given as a number. It represents both the width and height.
maxExtension: The maximum extension, or put in other words, the maximum step length.
goalBias: The goal bias fraction.
maxSamples: The maximum number of samples that the RRT planner should generate, to find a plan.
callback: A callback function that must be called at every iteration of the RRT algorithm, with the following parameters:
p, the current sampled point
q, the closest leaf in the tree to p
r, the extension point from q to r
t, the current tree grown so far.
The plan function should return the collision free path as an array of points with the first point in the list being the start and the last being the goal.
4 Design decisions, debugging, and visualizations
You will have to make several design decisions to successfully implement the RRT algorithm.
Here are some points you will have to think about:
Indicating the root node: What should be the value of the parent of the root node of the tree?
The callback function in the options object can be used to visualize the progress of the RRT algorithm, and to visually inspect for correctness. How can you implement this function? Note that the lines in a map do not have units. For visualization purposes, you can assume the point positions use the same units as the pixels in a canvas.
The goalBias and maxExtension values vary how the RRT algorithm performs on different maps. While we will not be evaluating your implementation for efficiency, you can play around with their values to see how it affects the resulting tree generated, and the final path found.
5 Visualization
To aid in visualization, there are a number of drawing functions built into the Lib220 library. Full documentation is available in the Lib220 Documentation file .To visualize the tree, you should make use of the callback function located in the options object. If you want to visualize the tree as it is created step by step, you will want to use the lib220.sleep(n) function which will pause your program for n milliseconds.
