Due: 4/13/06

Part 1. Implement a potential field path planner for an n link revolute
robot in the plane.  You will need to create the following:

   1. A function that accepts a robot specification that is a vector
      of link lengths, and a vector of joint angles.  The function
      should return the locations of the end of each of the links. You
      can assume the robot is rooted at the origin.

   2. A function that accepts a list of circular obstacles, each
      obstacle described by a location and a radius.  This function
      also accepts a robot configuration.  The result of this function
      is a repulsive potential (this is probably the hardest function
      to write).

   3. A function that accepts a goal point and a robot configuration,
      and returns a distance from the endpoint to the goal.


To complete the assignment, it suffices to now create an overall
simulation that:

   1. Accepts a list of obstacles and a goal point (ideally as a list
      from a text file).

   2. Uses a simple discrete search to go "downhill" in configuration
      space. 

   3. Outputs a set of waypoints that are the configurations and
      associated endpoints from the start to the goal.

You can acquire extra credit by:

   1. Implementing gradient search.

   2. Implementing a wavefront planner.

   3. Adding a graphical display of your robot.

   4. Anything else creative you can think of.