Skip to content

Commit

Permalink
First commit
Browse files Browse the repository at this point in the history
  • Loading branch information
guimeira committed Apr 27, 2016
0 parents commit ea5fa4f
Show file tree
Hide file tree
Showing 31 changed files with 2,198 additions and 0 deletions.
3 changes: 3 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
.idea/
target/
RRTControl.iml
60 changes: 60 additions & 0 deletions README
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
# RRT Control
![RRT Control](screenshot.png)

This application was developed as a project for the RBE-502 course at the [Worcester Polytechnic Institute].

It is a Java implementation of the method proposed by Romain Pepy, Alain Lambert and Hugues Mounier in the paper [Path Planning using a Dynamic Vehicle Model]. It uses Rapidly-Exploring Random Trees (RRT) with a dynamic model of a car known as *bicycle model* to generate more realistic paths for a non-holonomic car-like robot. Differential equations are integrated using a Runge-Kutta method and collision checking is done using the Separating Axis Theorem.

This application includes a GUI that lets you draw obstacles and set the start and end positions of the car, as well as the parameters of the algorithm and the vehicle model.

## Using the application

### Building
The application can be built by Maven. The command

```
mvn package
```

will build an executable jar that can be run with

```
java -jar <name-of-the-jar>
```

### Binaries
You can also download the precompiled binaries at the [Releases] page.

### Usage
The interface of the application is very simple. The yellow car is the starting position. The red car is the goal position. Use the **left mouse button** to create obstacles and to move the existing ones and the start and goal positions. Use the **right mouse button** to rotate obstacles and the start and goal positions. Use the **middle mouse button** to delete obstacles.

You can save and load your settings using the **Load** and **Save** buttons. A sample configuration file comes with the application. Configuration files are generated using Java serialization and should not be modified by hand.

The panel on the right can be used to set the parameters of the algorithm. There is **no** validation of those parameters, so make sure you provide valid values for a higher chance of getting a reasonable result back. Here is a short description of each parameter:

**Car parameters:**
* **Speed:** how fast the car moves forward
* **Width and Length:** the dimensions of the car

**Planner parameters:**
* **Reach goal threshold:** threshold that determines when a certain position is considered to be the goal. Smaller values make the algorithm take longer to finish (or not finish at all) and higher values give a final state that is not very close to the desired goal.
* **Time increment:** time step used for integration. Smaller values give a more precise output, but take longer. You probably should not use values bigger than 0.2. For higher speeds, use even smaller values.
* **Max. iterations:** maximum number of iterations of the algorithm, before it gives up.
* **Min. steering angle:** minimum steering angle the algorithm will try to use. Probably something around -60 should be ok.
* **Max. steering angle:** the maximum steering angle. Values around 60 should be ok.
* **Try goal probability:** goal bias of the RRT. It is the probability of the algorithm trying to move directly to the goal. Smaller values make the algorithm explore a wider area quickly and higher values make it move faster to the goal, however, if this value is too high and there are obstacles on the way, it may never find a path around the obstacles. It is a probability, so it should always be between 0 and 1.
* **Random seed:** seed for the pseudo-random number generator. When set to zero, it uses a random seed. Use other values to achieve repeatable results from the algorithm.

**Dynamic Model parameters:**
* **Cornering stiffness:** a constant that is related to the tire. You can specify a different value for the front and read wheels.
* **Mass:** the mass of the vehicle
* **Inertia:** the inertia of the vehicle

## Future work
This application can be improved in several ways. New models can be added easily by implementing the `CarModel` interface. Support for different variations of the RRT would also be nice, but adding it would take a little bit more time. Finally, the nearest neighbor search is a simple naive search, which makes the algorithm much slower as the tree gets bigger. Using a KD-tree for the search would probably make it handle large numbers of nodes much better.

I probably will not add any of those, but I will gladly accept pull requests with new features or bug fixes.

[Worcester Polytechnic Institute]: http://www.wpi.edu
[Path Planning using a Dynamic Vehicle Model]: http://www.cs.cmu.edu/afs/cs/Web/People/motionplanning/reading/PlanningforDynamicVeh-1.pdf
[Releases]: http://github.com/guimeira/rrtcontrol/releases
59 changes: 59 additions & 0 deletions pom.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
<modelVersion>4.0.0</modelVersion>

<groupId>com.guimeira</groupId>
<artifactId>rrtcontrol</artifactId>
<version>1.0-SNAPSHOT</version>
<packaging>jar</packaging>

<name>rrtcontrol</name>
<url>http://maven.apache.org</url>

<properties>
<project.build.sourceEncoding>UTF-8</project.build.sourceEncoding>
</properties>

<build>
<plugins>
<plugin>
<groupId>org.apache.maven.plugins</groupId>
<artifactId>maven-compiler-plugin</artifactId>
<version>2.3.2</version>
<configuration>
<source>1.8</source>
<target>1.8</target>
</configuration>
</plugin>
<plugin>
<artifactId>maven-assembly-plugin</artifactId>
<configuration>
<archive>
<manifest>
<mainClass>com.guimeira.rrtcontrol.App</mainClass>
</manifest>
</archive>
<descriptorRefs>
<descriptorRef>jar-with-dependencies</descriptorRef>
</descriptorRefs>
</configuration>
<executions>
<execution>
<phase>package</phase>
<goals>
<goal>single</goal>
</goals>
</execution>
</executions>
</plugin>
</plugins>
</build>

<dependencies>
<dependency>
<groupId>org.apache.commons</groupId>
<artifactId>commons-lang3</artifactId>
<version>3.4</version>
</dependency>
</dependencies>
</project>
Binary file added screenshot.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
11 changes: 11 additions & 0 deletions src/main/java/com/guimeira/rrtcontrol/App.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
package com.guimeira.rrtcontrol;

import com.guimeira.rrtcontrol.algorithm.*;
import com.guimeira.rrtcontrol.gui.MainWindow;
import org.apache.commons.lang3.Range;

public class App {
public static void main(String[] args) {
new MainWindow();
}
}
16 changes: 16 additions & 0 deletions src/main/java/com/guimeira/rrtcontrol/algorithm/CarModel.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
package com.guimeira.rrtcontrol.algorithm;

import java.util.Random;

public interface CarModel {
String[] getParameterNames();
String getModelName();
void configure(CarParameters carParameters, double... parameters);
Vector derivatives(Vector state, double input);
Point getPosition(Vector state);
double getTheta(Vector state);
double getWidth();
double getLength();
Vector randomState(Random randomGen, int width, int height);
Vector positionState(Point position, double theta);
}
25 changes: 25 additions & 0 deletions src/main/java/com/guimeira/rrtcontrol/algorithm/CarParameters.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
package com.guimeira.rrtcontrol.algorithm;

public class CarParameters {
private double speed;
private double width;
private double length;

public CarParameters(double speed, double width, double length) {
this.speed = speed;
this.width = width;
this.length = length;
}

public double getSpeed() {
return speed;
}

public double getWidth() {
return width;
}

public double getLength() {
return length;
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
package com.guimeira.rrtcontrol.algorithm;

public interface ClosestCriteria<T> {
double evaluate(T t1, T t2);
}
113 changes: 113 additions & 0 deletions src/main/java/com/guimeira/rrtcontrol/algorithm/DynamicModel.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,113 @@
package com.guimeira.rrtcontrol.algorithm;

import java.util.Random;

public class DynamicModel implements CarModel {
private double speed;
private double length;
private double width;
private double cf, cr;
private double mass;
private double inertia;
private double lf, lr;

private static final int STATE_VY = 0;
private static final int STATE_R = 1;
private static final int STATE_X = 2;
private static final int STATE_Y = 3;
private static final int STATE_THETA = 4;

@Override
public String[] getParameterNames() {
return new String[] {
"Front cornering stiffness",
"Rear cornering stiffness",
"Mass",
"Inertia"
};
}

@Override
public String getModelName() {
return "Dynamic Model";
}

@Override
public void configure(CarParameters carParameters, double... parameters) {
this.speed = carParameters.getSpeed();
this.width = carParameters.getWidth();
this.length = carParameters.getLength();
this.cf = parameters[0];
this.cr = parameters[1];
this.mass = parameters[2];
this.inertia = parameters[3];
this.lf = this.lr = length/2;
}

@Override
public Vector derivatives(Vector state, double input) {
double vy = state.getValue(STATE_VY);
double r = state.getValue(STATE_R);
double x = state.getValue(STATE_X);
double y = state.getValue(STATE_Y);
double theta = state.getValue(STATE_THETA);

double cosInput = Math.cos(input);
double cosTheta = Math.cos(theta);
double sinTheta = Math.sin(theta);

double a = -(cf*cosInput+cr)/(mass*speed);
double b = (-lf*cf*cosInput+lr*cr)/(mass*speed)-speed;
double c = (-lf*cf*cosInput+lr*cr)/(inertia*speed);
double d = -(lf*lf*cf*cosInput+lr*lr*cr)/(inertia*speed);
double e = cf*cosInput/mass;
double f = lf*cf*cosInput/inertia;

double vyDot = a*vy + c*r + e*input;
double rDot = b*vy + d*r + f*input;
double xDot = speed*cosTheta - vy*sinTheta;
double yDot = speed*sinTheta + vy*cosTheta;
double thetaDot = r;

return new Vector(vyDot, rDot, xDot, yDot, thetaDot);
}

@Override
public double getWidth() {
return width;
}

@Override
public double getLength() {
return length;
}

@Override
public Point getPosition(Vector state) {
return new Point(state.getValue(STATE_X), state.getValue(STATE_Y));
}

@Override
public double getTheta(Vector state) {
return state.getValue(STATE_THETA);
}

@Override
public Vector randomState(Random randomGen, int width, int height) {
double sX = randomGen.nextDouble()*width;
double sY = randomGen.nextDouble()*height;
double sTheta = randomGen.nextDouble()*2*Math.PI;

return new Vector(0, 0, sX, sY, sTheta);
}

@Override
public Vector positionState(Point position, double theta) {
return new Vector(0, 0, position.getX(), position.getY(), theta);
}

@Override
public String toString() {
return getModelName();
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
package com.guimeira.rrtcontrol.algorithm;

public interface EdgeIterationCallback<T,U> {
void process(T from, T to, U content);
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
package com.guimeira.rrtcontrol.algorithm;

import java.util.List;
import java.util.Random;

public class KinematicModel implements CarModel {
private double speed;
private double length;
private double width;

private static final int STATE_X = 0;
private static final int STATE_Y = 1;
private static final int STATE_THETA = 2;

@Override
public String getModelName() {
return "Kinematic Model";
}

@Override
public String[] getParameterNames() {
return new String[] {};
}

@Override
public void configure(CarParameters carParameters, double... parameters) {
this.speed = carParameters.getSpeed();
this.width = carParameters.getWidth();
this.length = carParameters.getLength();
}

@Override
public Vector derivatives(Vector state, double input) {
double theta = state.getValue(STATE_THETA);
double xDot = speed*Math.cos(theta);
double yDot = speed*Math.sin(theta);
double thetaDot = speed/length*Math.tan(input);
return new Vector(xDot,yDot,thetaDot);
}

@Override
public double getWidth() {
return width;
}

@Override
public double getLength() {
return length;
}

@Override
public Point getPosition(Vector state) {
return new Point(state.getValue(STATE_X), state.getValue(STATE_Y));
}

@Override
public double getTheta(Vector state) {
return state.getValue(STATE_THETA);
}

@Override
public Vector randomState(Random randomGen, int width, int height) {
double sX = randomGen.nextDouble()*width;
double sY = randomGen.nextDouble()*height;
double sTheta = randomGen.nextDouble()*2*Math.PI;

return new Vector(sX, sY, sTheta);
}

@Override
public Vector positionState(Point position, double theta) {
return new Vector(position.getX(), position.getY(), theta);
}

@Override
public String toString() {
return getModelName();
}
}
Loading

0 comments on commit ea5fa4f

Please sign in to comment.