Created new KDL kinematics plugin that uses a cache