fixed wrong starting foot bug for second trajectory