2.5D footstep planner for a bipedal robot