minor footstep tweaks
Improve step trimming to fix poor behavior when final steps are far apart