# Project Description:

My project consists of a worm that is made up of 4 links.  The worm has three degrees of freedom. It can move in the x direction, y direction and it can rotate.
The objective of the worm is to move around the obstacles to get to the apple. The apples were randomly placed in the scene. After it has reached the first apple it then leaves from that spot to go to the second apple.
The difficult part of this project is the worm.  With a worm, it is difficult to escape from local minimums without the worm colliding with itself.

# Approach:

I used Open GL instead of Leda for this project.  I used half-plane primatives for my collision detection.
My original path planning algorithm was a variation of a single Rapidly-Exploring Random Tree. This was not a good algorithm for this  project because the motion was random and the worm would crash into itself.
Also the path was not optimal at all. The path is shown in  yellow. The worm is located at the bottom of the screen. The worm does not actually move until the goal is reached. This method the worm did reach the goal.

My next solution to the problem was to try to implement different metrics along with the variation for the Rapidly Exploring Random Tree in order to reach the goal.
The first metric I chose was to pick five points at random. Then I would check to see which point was closest to the goal. I would choose that point and then use that point to pick the next 5 random points. This metric only worked for very simple examples.  It would get caught in local minimums. The following picture shows an example of this.

It was very difficult to try to come up with an algorithm that would allow the worm to come out of the local minimum. The worm could not turn over 90 degrees on either side otherwise it would run into itself. Since the biggest difficulty was getting the worm out away from the obstacle. The next thing I tried was to calculate the distance the worm was from the obstacles. If the distance was less than so many units (15) and the goal was not close by (more than 20 units away) then that point would not be selected even if it was the closest to the goal.
This improved the performance of the program, but the program still did not work for all cases. This method just basically increased the area of the local minimum.

Other things that I tried that did not solve the problem was implementing a random walk for 30 steps when the worm was stuck in a local minimum. Again the problem was the worm being too close to the obstacles to be able to get far enough away from the local minimum.
The final method that I implemented was when the worm was only 15 units away from an obstacle in the x direction, I would have it move to the left 90 degrees (so that it would go up), and if the worm was only 15 units away from an in the y direction, I would  have it move to the right 90 degrees. This solved some of the local minimum problems.

## Conclusion:

The variation of the RRT that I implemented was not the most efficient algorithm that I could have used. The RRT with no biasis could find a path but it was not optimal at all.
Looking back, I should have used LEDA. That way it would have been easier to implement some different path planning algorithms that would have been more optimal, such as the visibility graph. That would have solved my problem of getting caught in the local minimum.
Another thing that I would have liked to do with this problem was to make it 3D. That way when the worm got caught in the local minimum. It could just climb the wall of the obstacle, go over it and continue to the goal.

# Examples:

Note: Movies may flicker because I took out a few frames so they wouldn't be so large.

First Example (first half)   After you click on the link. Click on the black box and the movie will run.
Second half of First Example

Second Example  (has three apples)

Third Example   This example shows how the worm follows the edge of the obstacle to escape the local minimum. When the worm is going from apple 1 to apple 2.

Code: