42 #include <urdf_model/model.h> 43 #include <urdf_parser/urdf_parser.h> 45 #include <kdl/frames_io.hpp> 51 #define ROS_DEBUG(...) fprintf(stdout, __VA_ARGS__); 52 #define ROS_ERROR(...) fprintf(stderr, __VA_ARGS__); 53 #define ROS_WARN(...) fprintf(stderr, __VA_ARGS__); 58 #include <urdf/urdfdom_compatibility.h> 87 case urdf::Joint::FIXED: {
90 case urdf::Joint::REVOLUTE: {
94 case urdf::Joint::CONTINUOUS: {
98 case urdf::Joint::PRISMATIC: {
103 ROS_WARN(
"Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
116 double kdl_mass = i->mass;
144 std::vector<urdf::LinkSharedPtr> children = root->child_links;
145 ROS_DEBUG(
"Link %s had %zu children", root->name.c_str(), children.size());
149 if (root->inertial) {
150 inert =
toKdl(root->inertial);
158 root->parent_joint->parent_to_joint_origin_transform), inert);
161 tree.
addSegment(sgm, root->parent_joint->parent_link_name);
164 for (
size_t i = 0; i < children.size(); i++) {
174 const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(file);
181 #if defined(HAS_ROS) && defined(HAS_URDF) 184 ROS_ERROR(
"Could not generate robot model");
196 const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDF(xml);
198 ROS_ERROR(
"Could not generate robot model");
207 ROS_ERROR(
"Could not parse the xml document");
211 tinyxml2::XMLPrinter printer;
212 xml_doc->Print(&printer);
220 ROS_ERROR(
"Could not parse the xml document");
224 std::stringstream ss;
232 if (!robot_model.getRoot()) {
236 tree =
KDL::Tree(robot_model.getRoot()->name);
239 if (robot_model.getRoot()->inertial) {
240 ROS_WARN(
"The root link %s has an inertia specified in the URDF, but KDL does not " 241 "support a root link with an inertia. As a workaround, you can add an extra " 242 "dummy link to your URDF.", robot_model.getRoot()->name.c_str());
246 for (
size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) {
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
KDL::Vector toKdl(urdf::Vector3 v)
static Rotation Quaternion(double x, double y, double z, double w)
bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree &tree)
KDL_PARSER_PUBLIC bool treeFromFile(const std::string &file, KDL::Tree &tree)
KDL_PARSER_PUBLIC bool treeFromString(const std::string &xml, KDL::Tree &tree)
URDF_EXPORT bool initParam(const std::string ¶m)
KDL_PARSER_PUBLIC bool treeFromParam(const std::string ¶m, KDL::Tree &tree)
RotationalInertia getRotationalInertia() const
bool addSegment(const Segment &segment, const std::string &hook_name)
KDL_PARSER_PUBLIC bool treeFromXml(const tinyxml2::XMLDocument *xml_doc, KDL::Tree &tree)