2D Dynamic Window Approach [1] Motion Planning algorithm written in C with Python Bindings.
Table of Contents
- Dynamic Window Approach
https://goktug97.github.io/dwa/
- Python >= 3.6
- cython
- numpy
- cv2 (Optional for the demo)
- SDL
- OpenGL
You can directly install Python bindings without compiling the library.
git clone https://github.com/goktug97/DynamicWindowApproach
cd DynamicWindowApproach
mkdir build
cd build
cmake ..
make
make install
# Optional: Build Demo
make demo
pip3 install dynamic-window-approach --user
git clone https://github.com/goktug97/DynamicWindowApproach
cd DynamicWindowApproach
python3 setup.py install --user
-
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
.
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.
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
-
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.
-
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 speed 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)
-
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)
-
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)
-
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)
-
struct Pose
- Point point
- Coordinate of the robot.
- float yaw
- Angle of the robot.
- Point point
-
class Pose
Pose(Point point, float yaw)
-
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)
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:
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:
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)
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:
-
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:
-
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:
-
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:
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)
-
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)
- 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
MIT License.