1. Implement a visibility graph planner for a two-dimensional polygonal robot and work space, remembering to construct and display the configuration space.

My implementation can be roughly divided into three components: computing the configuration space, creating the visibility graph, and planning within it.

Computing the Configuration Space
To compute the workspace of a polygonal robot in a polygonal workspace, we intuitively want to slide the robot around each obstacle, tracing out the boundary of the effective (i.e., configuration space) obstacle. I implemented the Star Algorithm for this purpose, which is described in Principles of Robot Motion and Steve LaValle's Planning Algorithms. Roughly, the algorithm works as follows:

• For each obstacle polygon and the robot polygon, compute edge normals:
  • Compute the outward-facing edge normal for obstacles
  • Compute the inward-facing edge normal for the robot polygon
• Compute the angle of the normal for each (e.g., atan2(the normal))
• Create merged list containing both robot and obstacle normals, sorted by this angle
  • Each edge in this list will form an edge of the configuration space obstacle
• For each edge, find the (contact-point) vertex on the opposite (obstacle or robot) polygon that this edge will slide across to carve out the configuration space obstacle edge
  • This is the vertex shared by the edges of the opposite polygon surrounding this edge in the sorted list
• Compute the equation of the line each edge lies on in the configuration space obstacle
  • This is the set of points p=(x,y) satisfying n DOT v, where v = p + cv - ev (where cv is the contact-point vertex for the edge and ev is a vertex on the original edge) and n is the outward-facing normal for the polygon. Note that v is negated if the edge for which we're computing the line equation came from an obstacle rather than the robot.
  • (See LaValle's book for an intuitive explanation of why this is the case)
• The vertices of the configuration space obstacle are now simply the intersection points of consecutive lines

One subtlety is that if the robot and a given obstacle have a parallel normal (when sliding the robot polygon around the obstacle, an edge is formed by sliding across an obstacle edge rather than a vertex), there will be redundant entries in the sorted edge list (and thus redundant line equations and thus an infinitude of intersections forming vertices of the configuration space obstacle). These can simply be detected and discarded.

Finally, to generate the appropriate boundaries in the workspace, I treat it as having four rectangles surrounding it (one above, below, etc.) that are then inflated like the others using this algorithm.

Computing the Visibility Graph
To compute the visibility graph, I iterate over all pairs of obstacle vertices testing for line of sight uninterrupted by another obstacle or the obstacle itself (e.g., if we're trying to test passing back through the obstacle). I special case allowing the edge along the perimeter of the currently-evaluated polygon that connects the currently-evaluated vertex to its two neighbors, subject to the constraint that it must still may not pass through any other obstacles. I of course also explicitly include the start and goal points as such vertices. Note that I disregard the vertices offered by convex corners of the work(/configuration) space since no optimal path will use them anyway.

Planning over the Visibility Graph
Once the visibility graph has been built, we may search it using any shortest-path graph algorithm, such as Dijkstra's or A*. I chose A* for this purpose, having just implemented it for the previous homework. I'll refrain from describing the algorithm here as the reader may refer to my previous submission.

In the following diagrams, the start is shown in green, and the goal is red. A right triangle with a vertical left edge and the origin at the bottom-left vertex is used as the robot model.

Successful run

Base map

Inflated map (c-space)

Visibility graph

Optimal path along graph

Unsuccessful run

Base map

Inflated map (c-space)

Visibility graph

...along which there lies no path between the start and goal!

Source code: Available here

This appears to compile and run fine on linux.andrew.cmu.edu, and should do the same on most other Linux systems. No attempt was made to target other OSes.