Wednesday, May 09, 2012

Graphics links post

Its been a while since I've done a link post, (almost five months) so time to catch up! From the demoscene and WebGL world we have: In games we have: In tools and research we have:
  • A link collection of all the SIGGRAPH course notes
  • PowerVR articles on everything from OpenRL (open ray tracing library) to PVR texture compression and parallax bumpmapping. A mini (PowerVR) version of ATI and nVidia's article and tools collection.
  • GNU Plotting covers tips and tricks for generating graphs and plots with GNU plot.
  • Autodesk's free photofly lets you create 3d models from photos.
  • Insight3D is an open source image based modeling (3D models from photos) software package.
  • openFrameworks is a cross-platform C++ toolkit for making realtime visual productions, interfacing to OpenGL, GLEW, FMOD, FreeType, Quicktime, etc.
  • FXGen is an open source procedural texture generator.
  • libnoise is an open source noise (e.g. perlin) generator.
  • Robert Schneider maintains a list of mesh generation software for all your triangulation and surface, grid, tetrahedron generation needs.
  • Cyril Crassin posted his thesis on GigaVoxels.
  • 3D voxel sculpting with 3d-coat.
  • Sculptris is 3D sculpting software, similar to Zbrush.
  • AN IMAging Library supports a number of file formats, and also simple image operations such as distance transforms.
  • Pedro Felzenszwalb has some image distance transform code
  • Sander van Rossen's open source Winforms library for node/graph based user interfaces.
And other interesting things: I'll leave you with the winning 64k intro from Revision:

Tuesday, May 08, 2012

Dynamic Window Algorithm motion planning

Most robots have a set of navigation algorithms for motion planning that execute at different frequencies, global path planners (e.g. A*, ~0.1 Hz), mid-level path deformation (e.g. elastic band, ~5Hz), and collision / obstacle avoidance algorithms (~20Hz), which will be the last step before actuator control.
For MAGIC 2010, we used the Dynamic Window Approach.
(Note the ROS navigation stack offers the same algorithm configurations, but of-course didn't exist at the time we had to develop the WAMbot codebase). There are three common approaches used for local trajectory planning:
  • Potential-field based, where each obstacle has an obstacle 'force field' for repelling the robot, and the goal has a attraction field. (A similar approach is 'Vector-fields', and Virtual Force Field)
  • Dynamics based, where the algorithm consider the robots dynamics in calculating a solution. (e.g. Velocity Obstacles and Dynamic Window Approach)
  • Sampling based, where various collision free states are sampled and then combined. (e.g. Reachability graph, Probabilistic roadmaps)
The Dynamic Window Approach is a velocity-based local planner that calculates the optimal collision-free ('admissible') velocity for a robot required to reach its goal. It translates a cartesian goal (x,y) into a velocity (v,w) command for a mobile robot.

There are two main goals, calculate a valid velocity search space, and select the optimal velocity. The search space is constructed from the set of velocities which produce a safe trajectory (i.e. allow the robot to stop before colliding), given the set of velocities the robot can achieve in the next time slice given its dynamics ('dynamic window'). The optimal velocity is selected to maximize the robots clearance, maximize the velocity and obtain the heading closest to the goal.

Its easier to explain if we look at the code first. In pseudo-code the DWA is:
 
BEGIN DWA(robotPose,robotGoal,robotModel)
   desiredV = calculateV(robotPose,robotGoal)
   laserscan = readScanner()
   allowable_v = generateWindow(robotV, robotModel)
   allowable_w  = generateWindow(robotW, robotModel)
   for each v in allowable_v
      for each w in allowable_w
      dist = find_dist(v,w,laserscan,robotModel)
      breakDist = calculateBreakingDistance(v)
      if (dist > breakDist)  //can stop in time
         heading = hDiff(robotPose,goalPose, v,w)
         clearance = (dist-breakDist)/(dmax - breakDist)
         cost = costFunction(heading,clearance, abs(desired_v - v))
         if (cost > optimal)
            best_v = v
            best_w = w
            optimal = cost
    set robot trajectory to best_v, best_w
END

Now to explain:
  1. First, we can calculate the desired velocity to the goal based on our current position, and the destination. (e.g. go fast if we are far away, slow if we are close. Use Equations of Motion, see Circular motion for a mobile robot).
  2. Select the allowable velocities (linear 'v', and angular 'w') given the vehicles dynamics, e.g. allowable_v ranges from the current velocity subtract the robots maximum deceleration * timeslice to the current velocity plus the robots maximum acceleration * timeslice, or more compact: [v-a.t,v+a.t], likewise for angular velocity.
  3. Search through all the allowable velocities
  4. For each velocity, determine the closest obstacle for the proposed robot velocity (i.e. collision detection along the trajectory)
  5. Determine if the distance to the closest obstacle is within the robots breaking distance. If the robot will not be able to stop in time, disregard this proposed robot velocity.
  6. Otherwise, the velocity is 'admissible', so we can now calculate the values required for the objective function. In our case, the robots heading and clearance.
  7. Calculate the 'cost' for the proposed velocity. If the cost is better than anything else so far, set this as our best option.
  8. Finally, set the robots desired trajectory to the best proposed velocity.
If you've read all the other posts, you should now know enough to implement your own mobile robot navigation system, or at least understand a bit more about the algorithms in the ROS navigation stack.