integrated pathplanner and footstep planning code