Dynamic Window Approach algorithm written in C with Python Bindings
Project description
Dynamic Window Approach
2D Dynamic Window Approach [1] Motion Planning algorithm written in C with Python Bindings.
Table of Contents
- Dynamic Window Approach
Online Demo
https://goktug97.github.io/dwa/
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
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
pip3 install dynamic-window-approach --user
Source
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 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 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 tobuild
directory and runmake demo
.
Documentation
While the 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. The functions accept built-in or numpy types. The functions create required classes inside for easy usage. For example a snippet from the planning function;
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
- Parameters:
-
class Config
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]
- Parameters:
-
class Velocity
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.
- Parameters:
-
class Point
Point(float x, float y)
PointCloud
-
struct PointCloud
- int size
- Number of points.
- Point *points
- Array of points.
- int size
-
class PointCloud
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.
- Point point
-
class Pose
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
- int nPossibleW:
- Number of angular velocities in the Dynamic Window
- float *possibleW:
- Array of angular velocities with resolution of Config.yawrateResolution
- int nPossibleV:
-
class DynamicWindow
DynamicWindow(tuple velocity, Config config)
Functions
planning
Calculates best linear and angular velocities given the state. Only required function to use this library.
-
C
Velocity planning (Pose pose, Velocity velocity, Point goal, PointCloud *pointCloud, Config config);
- Parameters:
- pose: Pose
- velocity: Velocity
- goal: Point
- pointCloud: PointCloud
- config: Config
- Parameters:
-
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
- Parameters:
createDynamicWindow
Given robot configuration and current velocities, calculates DynamicWindow. The memory is allocated dynamically inside of the function and must be freed using freeDynamicWindow function.
-
C
void createDynamicWindow(Velocity velocity, Config config, DynamicWindow **dynamicWindow);
- Parameters:
- velocity: Velocity
- config: Config
- dynamicWindow: DynamicWindow
- Parameters:
-
Python
DynamicWindow class is used to create a DynamicWindow instance.
dw = dwa.DynamicWindow(velocity, config): print(dw.possible_v, dw.possible_w)
- Parameters:
- velocity: tuple: (Linear Velocity, Angular Velocity)
- config: Config class
- Parameters:
freeDynamicWindow
Free dynamically allocated memory.
-
C
void freeDynamicWindow(DynamicWindow *dynamicWindow);
- Parameters:
- dynamicWindow: DynamicWindow
- Parameters:
-
Python
Handled by the DynamicWindow class. See below.
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
Pose motion(Pose pose, Velocity velocity, float dt);
-
Python
x, y, yaw = motion(pose, velocity, dt)
- Parameters:
- pose: tuple: (x, y, yaw)
- velocity: tuple: (Linear Velocity, Angular Velocity)
- Parameters:
calculateVelocityCost
-
C
float calculateVelocityCost(Velocity velocity, Config config);
-
Python
velocity_cost = calculate_velocity_cost(velocity, config)
- Parameters:
- velocity: tuple: (Linear Velocity, Angular Velocity)
- config: Config
- Parameters:
calculateHeadingCost
-
C
float calculateHeadingCost(Pose pose, Point goal);
-
Python
heading_cost = calculate_heading_cost(pose, goal)
- Parameters:
- pose: tuple: (x, y, yaw)
- goal: tuple: (x, y)
- Parameters:
calculateClearanceCost
-
C
float calculateClearanceCost(Pose pose, Velocity velocity, PointCloud *pointCloud, Config config);
- Parameters:
- pose: Pose
- velocity: Velocity
- pointCloud: PointCloud
- config: Config
- Parameters:
-
Python
clearance_cost = 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
- Parameters:
createPointCloud
Given a size, creates a PointCloud. Must be freed using freePointCloud.
-
C
PointCloud* createPointCloud(int size);
for (int i = 0; i < pointCloud->size; ++i) { pointCloud->points[i].x = 0.0 pointCloud->points[i].y = 0.0 }
- Parameters:
- size: int
- Parameters:
-
Python
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.
size = 600 point_cloud = np.zeros((size, 2), dtype=np.float32) point_cloud = dwa.PointCloud(point_cloud)
freePointCloud
-
C
void freePointCloud(PointCloud* pointCloud);
- Parameters:
- pointCloud: PointCloud
- Parameters:
-
Python
Handled by the PointCloud class. See below.
def __dealloc__(self): if self.thisptr is not NULL: cdwa.freePointCloud(self.thisptr)
References
- 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
License
MIT License.
Project details
Download files
Download the file for your platform. If you're not sure which to choose, learn more about installing packages.
Source Distribution
Hashes for dynamic-window-approach-1.1.1.tar.gz
Algorithm | Hash digest | |
---|---|---|
SHA256 | b25378068a289803fe38b7e6e505588bae7e1d3f75c0e393c62924574cbe4de7 |
|
MD5 | 151a52df52dc7b0054b2369e647f8ae5 |
|
BLAKE2b-256 | 3e649a27778487912d0c5e8df1a8df39718a1ee29c3eb1eb260472470388d6b5 |