very basic implementation of footstep planner in MICP class