Added functions for printing KDL chains