#include "URDFTools.hpp"
#include <urdf_model/link.h>
#include <stack>
#include "../tools/Logger.hpp"