AStarFootstepPlanner This is a simplified version of ihmc's footstep planning algorithm implemented with C++