Sean Hyde
16-735 Robotic Motion Planning
November 26, 2007
Homework #7

This is my implementation of the Probabilistic Roadmap Algorithm.

The algorithm generates (currently 100) random configurations of a 4 link robot by randomizing all four links to go from 0-2pi radians. It then attempts to connect each configuration to the k (currently 3) nearest points using a local planner. The distance metric used is the sum of squared distance of each of the angles, as this is what the local planner will use. The local planner sweeps all possible configurations between the two desired configurations. If it encounters an obstacle anywhere in the sweep, it reports failure.

GUI

The GUI is not being used this time due to time constraints, and trouble getting the final animation to display correctly. I do believe I can get it working in the future though. The code does, however, read in a .mat file created by the GUI so the user can still use it to create obstacles.

Graph Creation

The graph is created as it is described in the introduction. If all the links are plotted, the space looks rather cluttered, so only the endpoints are plotted and dotted lines connecting the endpoints show which configurations are neighbors. Note, that the metric for "closest" neighbors is NOT the euclidean distance of the endpoint of the arm and so "closeness" on this view is rather hard to verify. The algorithm stores a list of neighbors for each configuration, but only stores undirected edges. This means if a contains b as a neighbor, b will not store a. At the end of the creation phase, this information is passed to the Java framework used in Homework 5. To recap, this is a Dijkstra's algorithm and graph backbone I wrote a few years ago. It requires Java 1.5 or higher!

Graph Traversal

The planner uses Dijkstra's Algorithm (written in Java, see below) to find the shortest distance between the start and end nodes. The weights on the edges are the sum of squared difference of all of the angles (the same metric used to find nearest neighbors).

Java Implementation Details

The Java code is responsible for the graph search and planning. I wrote the code a couple years ago for a data structures class and modified it slightly (mostly adapting it to use floating point values instead of integer weights) to fit this assignment. It has been compiled with the 1.6 version of the JDK and may not work with versions of Matlab older than 2007b (7.5).

The Dijkstra planner will return a list of edges that is the shortest path between the start and the end locations. This list will be empty if no path exists.

This caused some issues as I tried to animate the arm moving as the Dijkstra planner would return the path backwards (easily fixed) and the edges in the correct order, but not necessarily the vertices in the correct order. I wrote a small function to clean this up.

Results:

Image showing randomly generated end locations and path of end from start to finish with sparse environment.

Video of robot executing path in sparse environment.

Image showing randomly generated end locations and path of end from start to finish with dense environment.

Video of robot executing path in dense environment.

Source Files: prm2.m     AbstractGraph.java     AdjListGraph.java     AdjMatrixGraph.java     Edge.java     Graph.java     GraphAlgorithms.java     MyPriorityQueue.java     Vertex.java