Snap footstep to plane for joy command