CIS 390 Fall 2015 Robotics: Planning and Perception Kostas Daniilidis Homework 6 Lab session: Thursday October 27, 2016 10:30 AM, 2016 Code submission: Friday October 28, 2016 23:59 PM 1 Introduction In this homework, your task is to implement the naive Voronoi diagram algorithm and to apply it to navigating around polygonal obstacles in a confined environment. Your second task is to improve your solution to Homework 5, using a smarter algorithm for choosing where to place nodes. Unlike in previous homeworks, where we marked you on whether or not the robot reached the goal, in this homework we will mark you on how fast it reaches the goal. This homework is a partner assignment. You only need to submit the code once. For your implementation of the Voronoi algorithm, you will be given start and goal points that represent the bottom-left and top-right corners of the rectangular environment that you will be allowed to move within. The input also includes a set of polygons that represent obstacles in the environment. The idea is to move from the start to the goal while staying as far away as possible from the obstacles and the walls of the confined environment. The approximation you are going to do is to consider only the corners of the polygons as input to the Voronoi algorithm, along with a sampling of the points on the walls. After producing the Voronoi diagram of those points, you should discard all edges that intersect with either the walls or the obstacles, and keep only the collision-free subset of vertices and edges. After producing the corresponding graph of Voronoi vertices and edges within the environment, you should connect the starting and the goal point to their closest points in the graph, and then apply one of the two algorithms from the previous homework (either Dijkstra or A*), to find the shortest path from start to goal. We ask you to return both the graph you produced before applying the shortest path algorithm, and the path of nodes you will follow as a 2D array of (x,y) coordinates. Please note that this is a different output format than on the A* homework. For a review of the algorithm for Voronoi diagrams, please see the relevant chapter here. Note: There are many implementations of the Voronoi diagram algorithm online (such as the scipy implementation). You may use these functions to compare to your implementations to verify that they are correct, but please do not use them in your implementations themselves. The purpose of this exercise is to implement the algorithm yourselves. 1
2 Implementing Voronoi 2.1 Naive Voronoi Diagram Algorithm For this homework, you will be implementing the naive approximation for generating Voronoi diagrams. This algorithm is described briefly in the first paragraph of section 7.2 of the reading, and we will elaborate on the steps here. Sample points along the boundary of the environment, placing them the specified interval distance apart, and combine this list of points with the list of obstacle points. The green dots in Figure 2 show what should be in this list. For every pair of points p 1 and p 2 : Compute the bisecting half-plane between p 1 and p 2 that includes p 1. To simplify this process, you can use the shapely library s Polygon object. On Ubuntu, shapely can be installed with the following commands: sudo apt-get install libgeos-dev sudo -H pip install shapely To represent a halfspace with a polygon, create a polygon that encompasses all points that are closer to p 1 than p 2, and that extends further out than the bounds of the environment. See Figure 1 for an example. The code to generate a shapely Polygon is below. Note that you need to list the corners in counter-clockwise or clockwise order. poly1 = Polygon ([ corner1, corner2, corner3...]) Figure 1: Example of using a polygon to represent a halfplane, where the current point is p 1, the comparison point is p 2, and the boundary is the bounds of the environment. Find the intersection of all of the generated polygons. To compute the intersection of two polygons, you can use the shapely function intersection: poly_ intersection = poly1. intersection ( poly2 ) 2
The vertices and edges of the final polygon will be vertices and edges in a region of the full Voronoi diagram. You can extract the vertices of the polygon with the following line of code: (x,y) = final_poly. exterior. coords.xy After extracting the vertices, you must remove any vertices that are beyond the boundaries of the environment or are within an obstacle, as well as any edges that collide with a wall. You can find vertices in obstacles either by using the shapely method.contains or.distance (using.distance will allow you to specify some distance from the polygons such that a point still lies within that polygon, in order to fix floating point errors). Finally, you must merge the edges and vertices of all the Voronoi regions to get the full Voronoi diagram. Note that some vertices may exist in multiple regions. Because of floating point errors you may find that vertices from two regions that should be in the position are not exactly the same, e.g. (24.999...95,30) and (25,30). As a result, instead of using == to compare two positions, you must check if the distance between two vertices is less than some small tolerance (such as 10 10 ). You may use the straightlinedistance function from the A* homework to compute distances. 2.2 Implementation You are provided two functions in the file VoronoiPath.py that you must implement. The autograder depends on this interface so you may not change it. Also, please do not change the filename. Note: We have not provided you with ShortestPath.py, RobotControl.py, or DiffDriveController.py. You will need to copy these files in from your submission to Homework 5. Here is the interface for the two functions you must implement. 3
def genvoronoi ( start, goal, bound_bl, bound_tr, polygons ): genvoronoi generates the voronoi diagram for a given environment with outer boundary and a set of inner polygons considered as obstacles. It will also connect a start and goal position to the diagram. This function will generate a graph where nodes are vertices of the voronoi diagram and edges are the edges of the diagram. Input : start : start position as a 2 x1 array goal : goal position as a 2 x1 array bound_ bl : a 2 x1 array representing the position of the bottom left bound_ tr : a 2 x1 array representing the position of the top right polygons : a list of Nx2 arrays that each define a 2D polygon with N vertices within the boundaries. Each polygon s vertices are given in a counter - clockwise manner. Output : a tuple (g, nodes ) graphvor : a Graph object where nodes are the vertices in a voronoi diagram given the boundaries and polygons and edges are the edges of the diagram obstacles : a Mx2 array that contains the (x, y) positions of each sampled wall point as well as each polygon point # Your code here - Main steps : # 1) Accumulate all the corners from the polygons. # 2) Add a sampling of the points from the walls of the environment. # 3) Run the naive Voronoi algorithm on this set of vertices to get the # Voronoi diagram. # 4) Discard edges and vertices that collide with obstacles. # 5) Add the " start " and " goal " positions to the graph, and connect them to # the closest nodes in the Voronoi diagram. graphvor = [] obstacles = [] return graphvor, obstacles 4
def navigateenvironment ( start, goal, bound_bl, bound_tr, polygons ): Finds paths through the environment by generating a Voronoi diagram and then plans the shortest path using A* or Dijkstra s Input : start : start position as a 2 x1 array goal : goal position as a 2 x1 array bound_ bl : a 2 x1 array representing the position of the bottom left bound_ tr : a 2 x1 array representing the position of the top right polygons : a list of Nx2 arrays that each define a 2D polygon with N vertices within the boundaries. Each polygon s vertices are given in a counter - clockwise manner. Output : a tuple (g, nodes ) path : a Nx2 numpy array with the shortest path from start to goal through the Voronoi graph in (x, y) coordinates in the order visited. NOTE : You must convert the output of your original Dijkstra s or A* code from indices to positions. graphvor : as output by genvoronoi obstacles : as output by genvoronoi ( graphvor, obstacles ) = getvoronoi ( start, goal, bound_bl, bound_tr, polygons ) path = [] return path, graphvor, obstacles 2.3 Visualization To check your algoritmh, you will want to visualize its results and compare them to what scipy outputs. Run VoronoiDemo.py to plot an environment and the Voronoi diagram that you code generates. Feel free to test out other environments besides the one provided. VoronoiDemo.py will generate two plots. The first will be the Voronoi diagram generated by the scipy API call. You can treat this as the true diagram. Note that scipy s implementation, unlike yours, will not remove line edges that fall inside obstacles. The second plot will be the Voronoi diagram generated by your own code. A sample can be seen in Figure 2. In this figure, green circles are obstacle points, red crosses and green lines are the Voronoi vertices and edges respectively, red lines are polygons and the blue line is the computed shortest path from the start to goal. 5
Figure 2: Example of a computed Voronoi diagram 3 Improving Robot s Path Finding In Homework 5, you created a graph by placing nodes in every free cell in the environment. In class you ve learned about superior algorithms for creating a graph with knowledge of the obstacles, and then planning a path to the goal. These include trapezoidal decomposition, visibility graphs, and Voronoi diagrams. Improve your RobotControl.py submission for Homework 5 by making use of one of these algorithms. You will need to modify the generate graph function with your improved algorithm. You may also want to change its method signature to take in pos init and pos goal as arguments. You can tune your controller for better performance. If you have not done so already, you should attempt to get your robot to orient toward the (i + 1)th position on its path by the time it reaches the ith position, as described in Homework 5. You can also experiment with the density of points in your graph, smoothing/resampling your final path, and how you make the robot cut corners. Please note there is a trade-off between speed and safety, and we will consider a failure if the robot hit any obstacle. You may want to have one safe and one aggressive implementation to use during the lab session. You can begin with your HW5 implementation; it should already be able to reach the goal. See by how much you can improve the time it takes to do it. 4 Deliverables The grade distribution for this project will be as follows: 10% for sampling the wall correctly and returning a correct points list (the full set of points that are given as input to your Voronoi algorithm). 50% for producing the correct Voronoi graph of the environment. This is divided into: 6
30% for correct implementation of the Voronoi algorithm (i.e. computing the correct set of vertices and edges for the Voronoi graph without any extra vertices or edges). 10% for applying the post-processing we described to the graph (e.g. rejecting colliding edges and vertices). 50% for improving your approach from Homework 5, and being able to solve more complex mazes in a reasonable amount of time. This is divided into: 25% for performance in simulation 25% for performance on the robot 5 Submission and Grading As stated before, everything is automatically graded. The autograder uses turnin to receive submissions. You will get an email shortly after submitting saying your message was received. After the autograder is done, you will also receive your results via email. Submissions are as follows: turnin -c cis390 -p hw6 -v *.py. More details are in this CIS520 description. 7