Package: dynamic-window-approach
Dynamic Window Approach2D Dynamic Window Approach[1]Motion Planning algorithm written in C with Python Bindings.Table of ContentsDynamic Window ApproachOnline DemoRequirementsPythonC Libraries (Optional for the Demo)InstallationCompile and Install C LibraryInstall Python BindingsPyPISourceUsageDocumentationStructs and ClassesRectConfigVelocityPointPointCloudPoseDynamicWindowFunctionsplanningcreateDynamicWindowfreeDynamicWindowmotioncalculateVelocityCostcalculateHeadingCostcalculateClearanceCostcreatePointCloudfreePointCloudReferencesLicenseOnline Demohttps://goktug97.github.io/dwa/RequirementsPythonPython >= 3.6cythonnumpycv2 (Optional for the demo)C Libraries (Optional for the Demo)SDLOpenGLInstallationYou can directly install Python bindings without compiling the library.Compile and Install C Librarygitclonehttps://github.com/goktug97/DynamicWindowApproachcdDynamicWindowApproach
mkdirbuildcdbuild
cmake..
make
makeinstall# Optional: Build DemomakedemoInstall Python BindingsPyPIpip3installdynamic-window-approach--userSourcegitclonehttps://github.com/goktug97/DynamicWindowApproachcdDynamicWindowApproach
python3setup.pyinstall--userUsageOnly function you need to run to plan the next move is theplanningfunction.  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 inexamplesfolder is the same demo but implemented using different libraries for
visualization.The Python example uses OpenCV and you can run it by executingpython3 demo.pyin the examples folder.The C example uses OpenGL and SDL and you can run it by executing./demoin
bin folder. The bin folder is created after the compile so if you didn't
compile the demo while installing the library. Go tobuilddirectory and runmake demo.DocumentationWhile theplanningfunction 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 ClassesIf you are using Python bindings, you don't need to use any of these
classes exceptConfig. The functions accept built-in or
numpy types. The functions create required classes inside for easy
usage. For example a snippet from theplanningfunction;cdeffloatx,y,yaw,v,w,gx,gycdefPointCloud_point_cloud=PointCloud(point_cloud)x,y,yaw=posev,w=velocitygx,gy=goalcdefPose_pose=Pose(Point(x,y),yaw)cdefVelocity_velocity=Velocity(v,w)cdefPoint_goal=Point(gx,gy)Structs are for CClasses are for PythonRectstructRectGiven 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.ConfigstructConfigParameters: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 weightclearance- floating-point clearance cost weightvelocity- floating-point velocity cost weightbase-RectclassConfigConfig(floatmax_speed,floatmin_speed,floatmax_yawrate,floatmax_accel,floatmax_dyawrate,floatvelocity_resolution,floatyawrate_resolution,floatdt,floatpredict_time,floatheading,floatclearance,floatvelocity,listbase)VelocitystructVelocityParameters:linearVelocity- floating-point linear velocity of the robot [m/s]angularVelocity- floating-point angular velocity of the robot [yaw/s]classVelocityVelocity(floatlinear_velocity,floatangular_velocity)PointstructPointParameters:x– floating-point x-coordinate of the point.y– floating-point y-coordinate of the point.classPointPoint(floatx,floaty)PointCloudstructPointCloudintsizeNumber of points.Point *pointsArray ofpoints.classPointCloudPointCloud(np.ndarray[np.float32_t,ndim=2]point_cloud)PosestructPosePointpointCoordinate of the robot.floatyawAngle of the robot.classPosePose(Pointpoint,floatyaw)DynamicWindowstructDynamicWindowintnPossibleV:Number of linear velocities in the Dynamic Window.float *possibleV:Array of linear velocities with resolution ofConfig.velocityResolutionintnPossibleW:Number of angular velocities in the Dynamic Windowfloat *possibleW:Array of angular velocities with resolution ofConfig.yawrateResolutionclassDynamicWindowDynamicWindow(tuplevelocity,Configconfig)FunctionsplanningCalculates best linear and angular velocities given the state. Only
required function to use this library.CVelocityplanning(Posepose,Velocityvelocity,Pointgoal,PointCloud*pointCloud,Configconfig);Parameters:pose:Posevelocity:Velocitygoal:PointpointCloud:PointCloudconfig:ConfigPythonlinear_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.float32config:ConfigcreateDynamicWindowGiven robot configuration and current velocities, calculatesDynamicWindow.
The memory is allocated dynamically inside of the function and must be freed usingfreeDynamicWindowfunction.CvoidcreateDynamicWindow(Velocityvelocity,Configconfig,DynamicWindow**dynamicWindow);Parameters:velocity:Velocityconfig:ConfigdynamicWindow:DynamicWindowPythonDynamicWindowclass 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:ConfigclassfreeDynamicWindowFree dynamically allocated memory.CvoidfreeDynamicWindow(DynamicWindow*dynamicWindow);Parameters:dynamicWindow:DynamicWindowPythonHandled by theDynamicWindowclass. See below.def__dealloc__(self):ifself.thisptrisnotNULL:cdwa.freeDynamicWindow(self.thisptr)motionGiven 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.CPosemotion(Posepose,Velocityvelocity,floatdt);Parameters:pose:Posevelocity:Velocitydt:float (seconds)Pythonx,y,yaw=motion(pose,velocity,dt)Parameters:pose:tuple: (x, y, yaw)velocity:tuple: (Linear Velocity, Angular Velocity)calculateVelocityCostCfloatcalculateVelocityCost(Velocityvelocity,Configconfig);Parameters:velocity:Velocityconfig:ConfigPythonvelocity_cost=calculate_velocity_cost(velocity,config)Parameters:velocity:tuple: (Linear Velocity, Angular Velocity)config:ConfigcalculateHeadingCostCfloatcalculateHeadingCost(Posepose,Pointgoal);Parameters:pose:Posegoal:PointPythonheading_cost=calculate_heading_cost(pose,goal)Parameters:pose:tuple: (x, y, yaw)goal:tuple: (x, y)calculateClearanceCostCfloatcalculateClearanceCost(Posepose,Velocityvelocity,PointCloud*pointCloud,Configconfig);Parameters:pose:Posevelocity:VelocitypointCloud:PointCloudconfig:ConfigPythonclearance_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.float32config:ConfigcreatePointCloudGiven a size, creates aPointCloud. Must be freed usingfreePointCloud.CPointCloud*createPointCloud(intsize);for(inti=0;i<pointCloud->size;++i){pointCloud->points[i].x=0.0pointCloud->points[i].y=0.0}Parameters:size:intPythonPointCloudclass 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=600point_cloud=np.zeros((size,2),dtype=np.float32)point_cloud=dwa.PointCloud(point_cloud)freePointCloudCvoidfreePointCloud(PointCloud*pointCloud);Parameters:pointCloud:PointCloudPythonHandled by thePointCloudclass. See below.def__dealloc__(self):ifself.thisptrisnotNULL:cdwa.freePointCloud(self.thisptr)ReferencesD. 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=12589http://ais.informatik.uni-freiburg.de/teaching/ss19/robotics/slides/19-pathplanning-long.pdfLicenseMIT License.