Cleaned up ikfast plugin and got grasp_filter_test working