kdl_parser provides C++ tools to construct a KDL tree from an XML robot representation in URDF. - View it on GitHub