kdl_parser and kdl_parser_py provide tools to construct a KDL tree from an XML robot representation in URDF. - View it on GitHub