vigir_footstep_planning_basics repository