goktug97/DynamicWindowApproach

The Dynamic Window Approach (DWA) planning algorithm written in C with Python Bindings

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
  • Requirements
    • Python
    • C Libraries (Optional for the Demo)
  • Installation
    • Compile and Install C Library
    • Install Python Bindings
      • PyPI
      • Source
  • Usage
  • Documentation
    • Structs and Classes
      • Rect
      • Config
      • Velocity
      • Point
      • PointCloud
      • Pose
      • DynamicWindow
    • Functions
      • planning
      • createDynamicWindow
      • freeDynamicWindow
      • motion
      • calculateVelocityCost
      • calculateHeadingCost
      • calculateClearanceCost
      • createPointCloud
      • freePointCloud
  • References
  • License

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 to build directory and run make 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 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

• 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]

• 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.

• class Point

  Point(float x, float y)

PointCloud

• struct PointCloud

  • int size
    • Number of points.
  • Point *points
    • Array of points.

• 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.

• 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

• 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

• 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

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

• 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

freeDynamicWindow

Free dynamically allocated memory.

• C

  void freeDynamicWindow(DynamicWindow *dynamicWindow);

  • Parameters:
    • dynamicWindow: DynamicWindow

• 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);

  • Parameters:
    • pose: Pose
    • velocity: Velocity
    • dt: float (seconds)

• Python

  x, y, yaw = motion(pose, velocity, dt)

  • Parameters:
    • pose: tuple: (x, y, yaw)
    • velocity: tuple: (Linear Velocity, Angular Velocity)

calculateVelocityCost

• C

  float calculateVelocityCost(Velocity velocity, Config config);

  • Parameters:
    • velocity: Velocity
    • config: Config

• Python

  velocity_cost = calculate_velocity_cost(velocity, config)

  • Parameters:
    • velocity: tuple: (Linear Velocity, Angular Velocity)
    • config: Config

calculateHeadingCost

• C

  float calculateHeadingCost(Pose pose, Point goal);

  • Parameters:
    • pose: Pose
    • goal: Point

• Python

  heading_cost = calculate_heading_cost(pose, goal)

  • Parameters:
    • pose: tuple: (x, y, yaw)
    • goal: tuple: (x, y)

calculateClearanceCost

• C

  float calculateClearanceCost(Pose pose, Velocity velocity, PointCloud *pointCloud, Config config);

  • Parameters:
    • pose: Pose
    • velocity: Velocity
    • pointCloud: PointCloud
    • config: Config

• 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

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

• 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

• Python

  Handled by the PointCloud class. See below.

  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.