Got KDL working for grasp filtering