Add sample to compare heuristic functions of footstep planning

![screenshot_from_2014-11-16 22 57 56](https://cloud.githubusercontent.com/assets/40454/5061673/453bb58c-6de4-11e4-8560-9917d3db7bad.png)