Support end coords offset

support end-coords offset in footstep planning and now it works with hrpsys.