Add dimensions of footsteps to the result of footstep planner