HW Set 3 + MP Set 3 Solution

Guidelines { Read Carefully! Please check each problem for problem-speci c instructions. For the submission of this machine problem assignment, you should create a top-level folder named with your NETID, (e.g., XYZ007 for a single person group and XYZ007-ABC999 for a two-person group). You should have the following folder structure if you are a single person group:




xyz007




xyz007/RRT




xyz007/RRT/rrt.py




When you are ready to submit, remove any extra les (e.g., some python interpreter will create .pyc les) that are not required and zip the entire folder. The zip le should also be named with your NETID as XYZ007.zip or XYZ007-ABC999.zip. For this particular assignment, the folder structure is already created for you in the accompanied zip le; you just need to rename the top folder as instructed.




You are required to write your program adhering to Python 3.7 standards. Speci cally, we will grade only using python 3.7.4. Beside the default libraries supplied in the standard Python distribution, you may use ONLY numpy and matplotlib libraries for this MP assignment.




As mentioned in class, you may form groups of up to two people. Only a single student needs to submit per group.




Only add/change code where you are told to!







Problem [100 + 100 points]. Implementation of a complete RRT planner for 2D.




Robot. You will be implementing the Rapidly-exploring Random Tree (RRT) algorithm for a convex polygonal translating robot. That is, the robot can move in any direction but will NOT rotate. The robot, in its local coordinate system, will be de ned as a list of clockwise arranged points, e.g., the three points




(0; 0); (0:1; 0:1); (0:2; 0)




would de ne the triangle as shown below.












(0:1; 0:1)
(0; 0)
(0:2; 0)



The robot’s con guration in the (global) workspace will be speci ed using a single point q = (qx; qy), which is the origin of the robot’s local frame. That is, the robot will be occupying the space bounded by the points




(qx; qy); (qx + 0:1; qy + 0:1); (qx + 0:2; qy)




Note that the robot is not xed to be the triangle provided above. It may be any convex polygon with up to six vertices.




Environment/workspace. The robot resides in a 10 10 region with the lower left corner being (0; 0) and the upper right corner being (10; 10). There are multiple polygonal obstacles in the region. The obstacles may be non-convex, e.g., you may have obstacles that look like the following non-convex polygon




Page 1 © Jingjin Yu Rutgers University

CS 460/560 HW Set 3 + MP Set 3 Due date: 1:00am























































The environment will contain 0+ such obstacles. You may assume that two obstacles will not intersect with each other and no obstacle will intersect the boundary of the environment.




The problem. The robot and obstacles are speci ed in an input le (e.g., robot env 01.txt). Each line in the le represents a list of clockwise arranged x-y coordinates that de nes a polygon.The rst line of the le de nes the robot and the rest of the lines are the obstacles, one obstacle per line. We have already parsed these for you in rrt.py.







A full problem is then speci ed by such a le plus the start and goal con guration of the robot, which is passed on the command line as




python rrt.py robot env 01.txt 1.0 2.0 8.5 7







This will print out problem on the command line.




python rrt.py robot env 01.txt 1.0 2.0 8.5 7 display







This will visualize the problem as well as print out problem on the command line. You will of course be given di erent inputs to test your program. The visualization should give you the gure on the right.




The tasks. You are given a skeleton le rrt.py, as already




mentioned above, to work with. As said, the current code takes in as arguments a le describing the robot and the polygonal obstacles, and coordinates for the start and goal con gurations. You are to implement the RRT algorithm following the steps listed below.




Generate an RRT without obstacles [50 points]. You are to implement the function growSimpleRRT(points)



The argument points is a dictionary of point IDs to tuples of xy-coordinates that you will use as samples, i.e., instead of generating your own random samples, you should use these as the random samples. A provided points may look like




f1: (5.2,6.7), 2: (9.2,2.3), ...g




In the function growSimpleRRT(points), you are to realize the following:




Grow an RRT with the root (5; 5) for len(points) iterations. The RRT should be stored as an adjacency list (map) that looks like



f1: [2, 5], 2: [1, 3, 4], ...g







Page 2 © Jingjin Yu Rutgers University
CS 460/560 HW Set 3 + MP Set 3 Due date: 1:00am










For each iteration, for the given point i from len(points), which has coordinates pi = (xi; yi) = points[i], you are to nd the nearest point on the existing tree to pi. This means that sometimes the nearest point can be somewhere on an edge of the existing RRT instead of at an end point, which then requires the addition of a new point to the RRT. When this happens, the existing RRT data structure needs to be updated to re ex this. The map points should also be updated to add the new point. As an example, in the gure given below, the closest point to pi on the existing (blue) RRT tree is the (red) point. This (red) point, in addition to pi must be added to the RRT data structure as well as the map points.





















pi




root = (5; 5)
















pi




root = (5; 5)









(c) Return the updated map of points and also the RRT that you just build.




Note that you do not need to consider the size of robot in this task. That is, you may treat the robot as a point robot for this task.




Implement a basic BFS/DFS search routine [25 points]. You are to implement a basic breadth rst or depth rst search function (your choice) with the signature



basicSearch(tree, start, goal)




in which tree is an adjacency list (map). start and goal are the indices of two points in the tree. Your function should return a list of points that is a path from start to goal, e.g.,




[start, ..., goal]




Visualization [25 points]. You are to implement a visualization function displayRRTandPath(points, tree, path, robotStart, robotGoal, polygons)



that draws the tree that you have just grown and a path on the tree. This can be a straight-forward modi cation from the visualization code from MP2. It should display the RRT that you’ve just grown. In fact, this task is probably best done before you proceed with the rst task as it helps you check whether you have done the rst task correctly. Please draw the tree in black and the path in orange. You should check whether the path is empty. Your function should also draw the problem if the arguments robotStart, robotGoal, polygons are not set to None.




Implement basic collision detection [30 + 20 points]. You are to implement a basic collision checking routine that detects whether the robot, at a given global coordinate, would collide with the boundary and obstacles in the environment. The function should have the following signature



isCollisionFree(robot, point, obstacles)









Page 3 © Jingjin Yu Rutgers University
CS 460/560 HW Set 3 + MP Set 3 Due date: 1:00am










in which robot is the polygon (as a list of tuples) for the robot, point is where the robot is at, and obstacles is a list of obstacle polygons. All polygons are given as clockwise oriented points, e.g., a polygon will look like




[(x1, y1), (x2, y2), ...]




You may assume the boundary of the workspace as given in the beginning, i.e., the workspace is a 10 10 area. You will be given 60% of credits on this task if your code works with convex obstacles and full credits if your code works with non-convex polygonal obstacles. You do not need to consider self-intersecting obstacles like the one below. Your function should return either True or False.




























Solve a real problem using RRT [50 points]. You are now to put everything together and implement a simpli ed RRT algorithm with the signature



RRT(robot, obstacles, startPoint, goalPoint)




Here, robot and obstacles are the same as in the function isCollisionFree. startPoint and goalPoint have the format (as a tuple)




(x, y)




You will now need to sample random points yourself for growing the RRT. By simpli ed RRT, we mean that you do not need to grow a partial edge; if a new edge that is generated causes collision with an obstacle, you may simply discard that new point and edge, and try another random point. Your function should display the RRT that you created and draw the path that you found, using your function displayRRTandPath. Your function should also return the map of points, the adjacency list (map), and the path, i.e.,




return points, tree, path




Note that you should not modify the main function in rrt.py. Of course, you may modify it during your implementation and testing but it should not be changed to contain any additional logic that you implemented. You may add helper functions as needed inside rrt.py.




Good luck!




















































Page 4 © Jingjin Yu Rutgers University