Fix bug of swing foot calculation and add reset orbit

(GaitGenerator.*) : Fix bug of swing foot calculation and add reset orbit