Remove KDL dependency