Homework #5

Visibility Graph

Part I: Building Configuration Space

I have used the Star Algorithm to compute the 2-dimensional configuration space Q starting from the workspace W and the robot shape R. If we assume that the robot and the obstacles in the workspace are convex polygons, the Star Algorithm computes the obstacles in the workspace using the normal vectors of each side of the polygons. I have used the Matlab function norm = c_norm(tang, dir) that returns the normal vector for each side of a polygon. The function is the same used in the Homework #3.

The algorithm sorts the sides of the robot and of one obstacle by increasing angles of normal vectors. Thereafter it builds the new convex polygon adding all the sides of the robot and of the obstacle in the calculated sequence. I define the vertex with the higher x-value as reference point on the robot and the vertex with the lower x-value as reference point on the obstacle. Since the robot’s reference point can touch the obstacle reference point by a given robot’s configuration q, the corresponding vertex of the obstacle in the configuration space must be at the position q.

Since the obstacles in the configuration space can overlap each other I have implemented an algorithm to find all the possible intersections. I have written a new Matlab function [v,X] = check_int(V, V_f) that checks whether two edges in the plane intersect (parameter v = 1), overlie (parameter v > 1) or do not have any common points (parameter v = 0). Subsequently, I can define new obstacles in the configuration space Q that have at most one common edge, which can never belong to the Visibility Graph.

When the program starts, push the button “Draw Obstacle” to draw an obstacle and set with the left mouse button the vertices of the obstacles within the white area (robot workspace W). To end the process click with the right mouse button somewhere. To draw the robot push the button “Draw Robot” and set with the left mouse button the vertices of the robot within the white area. To end the process, click somewhere with the right mouse button. A black point will be placed on the reference vertex of the robot. To calculate the configuration space push the button “Compute Q”. To set the end configuration, push the button “End Configuration”, and set with the left mouse button the point. To calculate the Visibility Graph and the path, push the button “Calculate Path”.

Part II: Building and searching the Visibility Graph

In order to generate the Visibility Graph I have implemented an algorithm that connects all vertices in the configuration to all other vertices to which they have line-of-sight. In order to check if the line-of-sight does not intersect any edge, I use the Matlab function [v,X] = check_int(V,V_f). Moreover, in order to check that a line-of-sight does not completely lie inside an obstacle I have used the implemented Matlab function inside = vertex_check(X1,Y1,X2,Y2), that checks whether an edge lies inside or outside an angle defined by two other edges.

After building the Visibility Graph, I have implemented the A* algorithm to search the shortest path between the start configuration and the goal configuration. I have implemented a Matlab function [path, error] = astarv(edg, q_start, q_goal) where there is no path connecting the start and the goal configuration if the parameter error is 1 (as shown in the picture below).

For the A* algorithm the following functions are defined:

·       star(n): represents the set of vertices that have line-of-sight with n;

·       c(n₁,n₂): represents the length of the line-of-sight connecting the vertices n₁ and n₂;

·       g(n): represents the total length of back pointer from n to q_start;

·       h(n): represents the heuristic cost function which returns the estimated cost of the shortest path from n to q_goal (I have used the Euclidean distance);

·       f(n)=g(n)+h(n): is the estimated cost of the shortest path from q_start to q_goal via n.

The flow of the A* algorithm is the same as in Homework #4.

I have implemented this algorithm because it was very interesting working on finding intersections between obstacles in the configuration space. In order to register a movie of the path planning, there is a check box “Movie” that has to be checked before pushing the button “Calculate Path”.

Homework files:

Here are to download all MATLAB files: Visibility.zip.

Here are to download the successful planning episode movies: Example1.avi, Example2.avi, and Example3.avi.

Here is to download the movie of the planning episode which has failed: Example4.avi.

Here is to download a pdf version of the report: Visibility.pdf.