Footstep planner to be integrated with MHA*