diff --git a/README.md b/README.md
index 7a576ca..c0b400c 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,509 @@
Dynamic Window Approach
===================================
-### License
+2D Dynamic Window Approach [[1]](#ref1) Motion Planning algorithm written in C with Python Bindings.
+
+
+
+
+**Table of Contents**
+
+- [Dynamic Window Approach](#dynamic-window-approach)
+ - [Requirements](#requirements)
+ - [Python](#python)
+ - [C Libraries (Optional for the Demo)](#c-libraries-optional-for-the-demo)
+ - [Installation](#installation)
+ - [Compile and Install C Library](#compile-and-install-c-library)
+ - [Install Python Bindings](#install-python-bindings)
+ - [PyPI](#pypi)
+ - [Source](#source)
+ - [Usage](#usage)
+ - [Documentation](#documentation)
+ - [Structs and Classes](#structs-and-classes)
+ - [Rect](#rect)
+ - [Config](#config)
+ - [Velocity](#velocity)
+ - [Point](#point)
+ - [PointCloud](#pointcloud)
+ - [Pose](#pose)
+ - [DynamicWindow](#dynamicwindow)
+ - [Functions](#functions)
+ - [planning](#planning)
+ - [createDynamicWindow](#createdynamicwindow)
+ - [freeDynamicWindow](#freedynamicwindow)
+ - [motion](#motion)
+ - [calculateVelocityCost](#calculatevelocitycost)
+ - [calculateHeadingCost](#calculateheadingcost)
+ - [calculateClearanceCost](#calculateclearancecost)
+ - [createPointCloud](#createpointcloud)
+ - [freePointCloud](#freepointcloud)
+ - [References](#references)
+ - [License](#license)
+
+
+
+## Requirements
+
+### Python
+
+* Python >= 3.6
+* cython
+* numpy
+* cv2 (Optional for the demo)
+
+### C Libraries (Optional for the Demo)
+
+* SDL
+* OpenGL
+
+## Installation
+
+You can directly install Python bindings without compiling the library.
+
+### Compile and Install C Library
+
+```bash
+git clone https://github.com/goktug97/DynamicWindowApproach
+cd DynamicWindowApproach
+mkdir build
+cd build
+cmake ..
+make
+make install
+# Optional: Build Demo
+make demo
+```
+
+### Install Python Bindings
+
+#### PyPI
+
+```bash
+pip3 install dynamic-window-approach --user
+```
+
+#### Source
+
+```bash
+git clone https://github.com/goktug97/DynamicWindowApproach
+cd DynamicWindowApproach
+python3 setup.py install --user
+```
+
+## Usage
+
+- Only function you need to run to plan the next move is the
+[planning](#planning) function. Rest of the code for both C and
+Python examples are just to create simulation environment and GUI.
+The 2 examples that you can find in
+[examples](https://github.com/goktug97/DynamicWindowApproach/blob/master/examples/)
+folder is the same demo but implemented using different libraries for
+visualization.
+
+- The Python example uses OpenCV and you can run it by executing `python3 demo.py`
+in the examples folder.
+
+- The C example uses OpenGL and SDL and you can run it by executing `./demo` in
+bin folder. The bin folder is created after the compile so if you didn't
+compile the demo while installing the library. Go to `build` directory and run
+`make demo`.
+
+- There is also a web application demo in
+a different repository which can be find at [DWA Web
+Demo](https://github.com/goktug97/DWAWebDemo).
+
+## Documentation
+
+While the [planning](#planning) function is the only function that a user needs
+to call for the planning, all of the functions are exposed to the user
+for both C and Python for no reasons.
+
+### Structs and Classes
+If you are using Python bindings, you don't need to use any of these
+classes except [Config](#config). The functions accept built-in or
+numpy types. The functions create required classes inside for easy
+usage. For example a snippet from the [planning](#planning) function;
+
+``` cython
+cdef float x, y, yaw, v , w, gx, gy
+cdef PointCloud _point_cloud = PointCloud(point_cloud)
+x, y, yaw = pose
+v, w = velocity
+gx, gy = goal
+cdef Pose _pose = Pose(Point(x, y), yaw)
+cdef Velocity _velocity = Velocity(v, w)
+cdef Point _goal = Point(gx, gy)
+```
+
+- Structs are for C
+- Classes are for Python
+
+#### Rect
+
+- *struct* Rect
+
+ * Given center of the robot is (0, 0)
+ * **Parameters**:
+ - **xmin** - floating-point minimum x-coordinate of the robot.
+ - **ymin** - floating-point minimum y-coordinate of the robot.
+ - **xmax** - floating-point maximum x-coordinate of the robot.
+ - **ymax** - floating-point maximum y-coordinate of the robot.
+
+#### Config
+
+- *struct* Config
+
+ * **Parameters**:
+ - **maxSpeed** - floating-point maximum linear speed robot can reach [m/s]
+ - **minSpeed** - floating-point minimum linea speed robot can fall [m/s]
+ - **maxYawrate** - floating-point maximum angular spped robot can reach [yaw/s]
+ - **maxAccel** - floating-point maximum linear acceleration robot can reach [m/ss]
+ - **maxdYawrate** - floating-point maximum angular acceleration robot can reach [yaw/ss]
+ - **velocityResolution** - floating-point linear speed resolution [m/s]
+ - **yawrateResolution** - floating-point angular speed resolution [m/s]
+ - **dt** - floating-point time change [s]
+ - **predictTime** - floating-point simulation time [s]
+ - **heading** - floating-point heading cost weight
+ - **clearance** - floating-point clearance cost weight
+ - **velocity** - floating-point velocity cost weight
+ - **base** - [Rect](#rect)
+
+- *class* Config
+
+ ``` python
+ Config(float max_speed, float min_speed,
+ float max_yawrate, float max_accel, float max_dyawrate,
+ float velocity_resolution, float yawrate_resolution, float dt,
+ float predict_time, float heading, float clearance, float velocity,
+ list base)
+ ```
+
+#### Velocity
+
+- *struct* Velocity
+
+ * **Parameters**:
+ - **linearVelocity** - floating-point linear velocity of the robot [m/s]
+ - **angularVelocity** - floating-point angular velocity of the robot [yaw/s]
+
+- *class* Velocity
+
+ ``` python
+ Velocity(float linear_velocity, float angular_velocity)
+ ```
+
+#### Point
+
+- *struct* Point
+
+ * **Parameters**:
+ - **x** – floating-point x-coordinate of the point.
+ - **y** – floating-point y-coordinate of the point.
+
+- *class* Point
+
+ ``` python
+ Point(float x, float y)
+ ```
+
+#### PointCloud
+
+- *struct* PointCloud
+
+ * int **size**
+ - Number of points.
+ * Point ***points**
+ - Array of [points](#point).
+
+- *class* PointCloud
+
+ ``` python
+ PointCloud(np.ndarray[np.float32_t, ndim=2] point_cloud)
+ ```
+
+#### Pose
+
+- *struct* Pose
+ * Point **point**
+ - Coordinate of the robot.
+ * float **yaw**
+ - Angle of the robot.
+
+- *class* Pose
+
+ ``` python
+ Pose(Point point, float yaw)
+ ```
+
+#### DynamicWindow
+
+- *struct* DynamicWindow
+ * int **nPossibleV**:
+ - Number of linear velocities in the Dynamic Window.
+ * float ***possibleV**:
+ - Array of linear velocities with resolution of [Config.velocityResolution](#config)
+ * int **nPossibleW**:
+ - Number of angular velocities in the Dynamic Window
+ * float ***possibleW**:
+ - Array of angular velocities with resolution of [Config.yawrateResolution](#config)
+
+- *class* DynamicWindow
+
+ ```python
+ DynamicWindow(tuple velocity, Config config)
+ ```
+
+### Functions
+
+#### planning
+
+Calculates best linear and angular velocities given the state. Only
+required function to use this library.
+
+- C
+
+ ``` c++
+ Velocity planning (Pose pose, Velocity velocity, Point goal, PointCloud *pointCloud, Config config);
+ ```
+
+ * **Parameters:**
+ - **pose:** [Pose](#pose)
+ - **velocity:** [Velocity](#velocity)
+ - **goal:** [Point](#point)
+ - **pointCloud:** [PointCloud](#pointcloud)
+ - **config:** [Config](#config)
+
+- Python
+
+ ``` python
+ linear_velocity, angular_velocity = planning(pose, velocity, goal, point_cloud, config)
+ ```
+
+ * **Parameters:**
+ - **pose:** tuple: (x, y, yaw)
+ - **velocity:** tuple: (Linear Velocity, Angular Velocity)
+ - **goal:** tuple: (x, y)
+ - **point_cloud:** Numpy Array of shape (N, 2) and type np.float32
+ - **config:** [Config](#config)
+
+#### createDynamicWindow
+
+Given robot configuration and current velocities, calculates [DynamicWindow](#dynamicwindow).
+The memory is allocated dynamically inside of the function and must be freed using
+[freeDynamicWindow](#freedynamicwindow) function.
+
+- C
+
+ ``` c++
+ void createDynamicWindow(Velocity velocity, Config config, DynamicWindow **dynamicWindow);
+ ```
+
+ * **Parameters:**
+ - **velocity:** [Velocity](#velocity)
+ - **config:** [Config](#config)
+ - **dynamicWindow:** [DynamicWindow](#dynamicwindow)
+
+- Python
+
+ [DynamicWindow](#dynamicwindow) class is used to create a DynamicWindow instance.
+
+ ``` python
+ dw = dwa.DynamicWindow(velocity, config):
+ print(dw.possible_v, dw.possible_w)
+ ```
+
+ * **Parameters:**
+ - **velocity:** tuple: (Linear Velocity, Angular Velocity)
+ - **config:** [Config](#config) class
+
+[Dynamic Window [2]](https://raw.githubusercontent.com/goktug97/DynamicWindowApproach/master/img/dynamic_window.jpg)
+
+#### freeDynamicWindow
+
+Free dynamically allocated memory.
+
+- C
+
+ ``` c++
+ void freeDynamicWindow(DynamicWindow *dynamicWindow);
+ ```
+
+ * **Parameters:**
+ - **dynamicWindow:** [DynamicWindow](#dynamicwindow)
+
+- Python
+
+ Handled by the [DynamicWindow](#dynamicwindow) class. See below.
+
+ ``` python
+ def __dealloc__(self):
+ if self.thisptr is not NULL:
+ cdwa.freeDynamicWindow(self.thisptr)
+
+ ```
+
+#### motion
+
+Given current position and velocities, calculates next position after
+given dt using differential drive motion model. Can be used to
+simulate motion in a simulated environment.
+
+- C
+
+ ``` c++
+ Pose motion(Pose pose, Velocity velocity, float dt);
+ ```
+
+ * **Parameters:**
+ - **pose:** [Pose](#pose)
+ - **velocity:** [Velocity](#velocity)
+ - **dt:** float (seconds)
+
+- Python
+
+ ``` python
+ x, y, yaw = motion(pose, velocity, dt)
+ ```
+
+ * **Parameters:**
+ - **pose:** tuple: (x, y, yaw)
+ - **velocity:** tuple: (Linear Velocity, Angular Velocity)
+
+#### calculateVelocityCost
+
+- C
+
+ ``` c++
+ float calculateVelocityCost(Velocity velocity, Config config);
+ ```
+
+ * **Parameters:**
+ - **velocity:** [Velocity](#velocity)
+ - **config:** [Config](#config)
+
+- Python
+
+ ``` python
+ velocity_cost = calculate_velocity_cost(velocity, config)
+ ```
+
+ * **Parameters:**
+ - **velocity:** tuple: (Linear Velocity, Angular Velocity)
+ - **config:** [Config](#config)
+
+#### calculateHeadingCost
+
+- C
+
+ ``` c++
+ float calculateHeadingCost(Pose pose, Point goal);
+ ```
+
+ * **Parameters:**
+ - **pose:** [Pose](#pose)
+ - **goal:** [Point](#point)
+
+- Python
+
+ ``` python
+ heading_cost = calculate_heading_cost(pose, goal)
+ ```
+
+ * **Parameters:**
+ - **pose:** tuple: (x, y, yaw)
+ - **goal:** tuple: (x, y)
+
+#### calculateClearanceCost
+
+- C
+
+ ``` c++
+ float calculateClearanceCost(Pose pose, Velocity velocity, PointCloud *pointCloud, Config config);
+ ```
+
+ * **Parameters:**
+ - **pose:** [Pose](#pose)
+ - **velocity:** [Velocity](#velocity)
+ - **pointCloud:** [PointCloud](#pointcloud)
+ - **config:** [Config](#config)
+
+- Python
+
+ ``` python
+ def calculate_clearance_cost(pose, velocity, point_cloud, config):
+ ```
+
+ * **Parameters:**
+ - **pose:** tuple: (x, y, yaw)
+ - **velocity:** tuple: (Linear Velocity, Angular Velocity)
+ - **point_cloud:** Numpy Array of shape (N, 2) and type np.float32
+ - **config:** [Config](#config)
+
+#### createPointCloud
+
+Given a size, creates a [PointCloud](#pointcloud). Must be freed using [freePointCloud](#freepointcloud).
+
+- C
+
+ ``` c++
+ PointCloud* createPointCloud(int size);
+ ```
+
+ ``` c++
+ for (int i = 0; i < pointCloud->size; ++i) {
+ pointCloud->points[i].x = 0.0
+ pointCloud->points[i].y = 0.0
+ }
+ ```
+
+ * **Parameters:**
+ - **size:** int
+
+- Python
+
+ [PointCloud](#pointcloud) class is used to create a PointCloud instance. All
+ functions in python accepts numpy array instead of PointCloud instance. The
+ PointCloud instance is created inside of the function.
+
+ ``` python
+ size = 600
+ point_cloud = np.zeros((size, 2), dtype=np.float32)
+ point_cloud = dwa.PointCloud(point_cloud)
+ ```
+
+#### freePointCloud
+
+- C
+
+ ``` c++
+ void freePointCloud(PointCloud* pointCloud);
+ ```
+
+ * **Parameters:**
+ - **pointCloud:** [PointCloud](#pointcloud)
+
+- Python
+
+ Handled by the [PointCloud](#pointcloud) class. See below.
+
+ ``` python
+ def __dealloc__(self):
+ if self.thisptr is not NULL:
+ cdwa.freePointCloud(self.thisptr)
+ ```
+
+## References
+
+
+
+1. D. Fox, W. Burgard and S. Thrun, "The dynamic window approach to
+collision avoidance," in IEEE Robotics & Automation Magazine, vol. 4,
+no. 1, pp. 23-33, March 1997. doi: 10.1109/100.580977 URL:
+http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=580977&isnumber=12589
+
+
+
+2. http://ais.informatik.uni-freiburg.de/teaching/ss19/robotics/slides/19-pathplanning-long.pdf
+
+## License
MIT License.
diff --git a/dwa.gif b/dwa.gif
new file mode 100644
index 0000000..a519b5e
Binary files /dev/null and b/dwa.gif differ
diff --git a/examples/demo.c b/examples/demo.c
index b28ef62..ad1a652 100644
--- a/examples/demo.c
+++ b/examples/demo.c
@@ -12,6 +12,11 @@ void renderRectangle(Rect rect, Pose pose);
void renderPointCloud(PointCloud *pointCloud, int size);
void drawCircle(GLfloat x, GLfloat y, GLfloat radius);
+/*
+NOTE: 1 Unit = 0.1 m
+That's why everything is multiplied or divided by 10.
+*/
+
void initGL(void) {
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
@@ -28,7 +33,7 @@ void drawCircle(GLfloat x, GLfloat y, GLfloat radius){
glVertex2f(x, y);
for (int i = 0; i <= nTriangle; i++) {
glVertex2f(x + (radius * cos(i * M_PI2 / nTriangle)),
- y + (radius * sin(i * M_PI2 / nTriangle)));
+ y + (radius * sin(i * M_PI2 / nTriangle)));
}
glEnd();
}
@@ -45,8 +50,8 @@ void renderPointCloud(PointCloud *pointCloud, int size){
void renderRectangle(Rect rect, Pose pose) {
glPushMatrix();
glTranslatef(pose.point.x*10.0,
- pose.point.y*10.0,
- 0.0f);
+ pose.point.y*10.0,
+ 0.0f);
glRotatef(pose.yaw * 180.0/M_PI, 0, 0, 1);
glBegin(GL_POLYGON);
glColor3f(1, 0, 0);
@@ -117,44 +122,50 @@ int main() {
running = 0;
break;
case SDL_KEYDOWN:
- switch (sdlEvent.key.keysym.sym) {
- case SDLK_ESCAPE:
- running = 0;
- case SDLK_r:
- freePointCloud(pointCloud);
- pointCloud = createPointCloud(100);
- currentIdx = 0;
- pose.point.x = 0.0;
- pose.point.y = 0.0;
- pose.yaw = 0.0;
- }
- break;
+ switch (sdlEvent.key.keysym.sym) {
+ case SDLK_ESCAPE:
+ running = 0;
+ case SDLK_r:
+ freePointCloud(pointCloud);
+ pointCloud = createPointCloud(100);
+ currentIdx = 0;
+ pose.point.x = 0.0;
+ pose.point.y = 0.0;
+ pose.yaw = 0.0;
+ velocity.linearVelocity = 0.0;
+ velocity.angularVelocity = 0.0;
+ }
+ break;
case SDL_MOUSEBUTTONDOWN:
- drawing = 1;
+ drawing = 1;
break;
case SDL_MOUSEBUTTONUP:
- drawing = 0;
- break;
+ drawing = 0;
+ break;
case SDL_MOUSEMOTION:
- if (drawing) {
- pointCloud->points[currentIdx].x = (sdlEvent.button.x - RESOLUTION_WIDTH/2.0)/10.0;
- pointCloud->points[currentIdx].y = (-sdlEvent.button.y + RESOLUTION_HEIGHT/2.0)/10.0;
- currentIdx++;
- if (!(currentIdx % 100)) {
- tmpPointCloud = createPointCloud(currentIdx);
- memcpy(tmpPointCloud->points, pointCloud->points, currentIdx*sizeof(Point));
- freePointCloud(pointCloud);
- pointCloud = createPointCloud(currentIdx+100);
- memcpy(pointCloud->points, tmpPointCloud->points, currentIdx*sizeof(Point));
- freePointCloud(tmpPointCloud);
- }
- } else {
- goal.x = (sdlEvent.button.x - RESOLUTION_WIDTH/2.0)/10.0;
- goal.y = (-sdlEvent.button.y + RESOLUTION_HEIGHT/2.0)/10.0;
- glColor3f(0, 1, 0);
- drawCircle(goal.x*10, goal.y*10, 3);
- }
- break;
+ if (drawing) {
+ pointCloud->points[currentIdx].x = (sdlEvent.button.x -
+ RESOLUTION_WIDTH/2.0)/10.0;
+ pointCloud->points[currentIdx].y = (-sdlEvent.button.y +
+ RESOLUTION_HEIGHT/2.0)/10.0;
+ currentIdx++;
+ if (!(currentIdx % 100)) {
+ tmpPointCloud = createPointCloud(currentIdx);
+ memcpy(tmpPointCloud->points, pointCloud->points,
+ currentIdx*sizeof(Point));
+ freePointCloud(pointCloud);
+ pointCloud = createPointCloud(currentIdx+100);
+ memcpy(pointCloud->points, tmpPointCloud->points,
+ currentIdx*sizeof(Point));
+ freePointCloud(tmpPointCloud);
+ }
+ } else {
+ goal.x = (sdlEvent.button.x - RESOLUTION_WIDTH/2.0)/10.0;
+ goal.y = (-sdlEvent.button.y + RESOLUTION_HEIGHT/2.0)/10.0;
+ glColor3f(0, 1, 0);
+ drawCircle(goal.x*10, goal.y*10, 3);
+ }
+ break;
}
}
if (currentIdx) {
diff --git a/examples/demo.py b/examples/demo.py
index f94ff75..574547d 100644
--- a/examples/demo.py
+++ b/examples/demo.py
@@ -9,6 +9,7 @@
class Demo(object):
def __init__(self):
# 1 px = 0.1 m
+ # That's why everything is multiplied or divided by 10.
cv2.namedWindow('cvwindow')
cv2.setMouseCallback('cvwindow', self.callback)
self.drawing = False
diff --git a/img/dynamic_window.jpg b/img/dynamic_window.jpg
new file mode 100644
index 0000000..341a963
Binary files /dev/null and b/img/dynamic_window.jpg differ
diff --git a/src/dwa.c b/src/dwa.c
index f2d58ed..b65ec79 100644
--- a/src/dwa.c
+++ b/src/dwa.c
@@ -89,14 +89,14 @@ calculateClearanceCost
x = -dx * cos(pPose.yaw) + -dy * sin(pPose.yaw);
y = -dx * -sin(pPose.yaw) + -dy * cos(pPose.yaw);
if (x <= config.base.xmax &&
- x >= config.base.xmin &&
- y <= config.base.ymax &&
- y >= config.base.ymin){
- return FLT_MAX;
+ x >= config.base.xmin &&
+ y <= config.base.ymax &&
+ y >= config.base.ymin){
+ return FLT_MAX;
}
r = sqrtf(dx*dx + dy*dy);
if (r < minr)
- minr = r;
+ minr = r;
}
time += config.dt;
}
@@ -105,7 +105,7 @@ calculateClearanceCost
Velocity
planning(Pose pose, Velocity velocity, Point goal,
- PointCloud *pointCloud, Config config){
+ PointCloud *pointCloud, Config config) {
DynamicWindow *dw;
createDynamicWindow(velocity, config, &dw);
Velocity pVelocity;
@@ -120,12 +120,13 @@ planning(Pose pose, Velocity velocity, Point goal,
pVelocity.angularVelocity = dw->possibleW[j];
pPose = motion(pPose, pVelocity, config.predictTime);
cost =
- config.velocity * calculateVelocityCost(pVelocity, config) +
- config.heading * calculateHeadingCost(pPose, goal) +
- config.clearance * calculateClearanceCost(pose, pVelocity, pointCloud, config);
+ config.velocity * calculateVelocityCost(pVelocity, config) +
+ config.heading * calculateHeadingCost(pPose, goal) +
+ config.clearance * calculateClearanceCost(pose, pVelocity,
+ pointCloud, config);
if (cost < total_cost) {
- total_cost = cost;
- bestVelocity = pVelocity;
+ total_cost = cost;
+ bestVelocity = pVelocity;
}
}
}