Added changes from ikfast_repo