Fix a few minor bugs in footstep and walking planning