Put estimated feet position pos_feet_ into currentFootstep_ instead of using desired position footsteps_