MHMP with Modifications.
Find the original code
http://www.cs.washington.edu/ai/Mobile_Robotics/projects/hmp/
