-
Notifications
You must be signed in to change notification settings - Fork 6
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
0 parents
commit ea5fa4f
Showing
31 changed files
with
2,198 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,3 @@ | ||
.idea/ | ||
target/ | ||
RRTControl.iml |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> |
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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
16
src/main/java/com/guimeira/rrtcontrol/algorithm/CarModel.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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
25
src/main/java/com/guimeira/rrtcontrol/algorithm/CarParameters.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | ||
} | ||
} |
5 changes: 5 additions & 0 deletions
5
src/main/java/com/guimeira/rrtcontrol/algorithm/ClosestCriteria.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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
113
src/main/java/com/guimeira/rrtcontrol/algorithm/DynamicModel.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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(); | ||
} | ||
} |
5 changes: 5 additions & 0 deletions
5
src/main/java/com/guimeira/rrtcontrol/algorithm/EdgeIterationCallback.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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); | ||
} |
79 changes: 79 additions & 0 deletions
79
src/main/java/com/guimeira/rrtcontrol/algorithm/KinematicModel.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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(); | ||
} | ||
} |
Oops, something went wrong.