bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml) { clear(); if (!robot_xml || robot_xml->ValueStr() != "robot") { logError("Could not find the 'robot' element in the xml file"); return false; } // get the robot name const char *name = robot_xml->Attribute("name"); if (!name) logError("No name given for the robot."); else { name_ = std::string(name); boost::trim(name_); if (name_ != urdf_model.getName()) logError("Semantic description is not specified for the same robot as the URDF"); } loadVirtualJoints(urdf_model, robot_xml); loadGroups(urdf_model, robot_xml); loadGroupStates(urdf_model, robot_xml); loadEndEffectors(urdf_model, robot_xml); loadLinkSphereApproximations(urdf_model, robot_xml); loadDisabledCollisions(urdf_model, robot_xml); loadPassiveJoints(urdf_model, robot_xml); return true; }
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree, const bool consider_root_link_inertia) { if (consider_root_link_inertia) { //For giving a name to the root of KDL using the robot name, //as it is not used elsewhere in the KDL tree std::string fake_root_name = "__kdl_import__" + robot_model.getName()+"__fake_root__"; std::string fake_root_fixed_joint_name = "__kdl_import__" + robot_model.getName()+"__fake_root_fixed_joint__"; tree = Tree(fake_root_name); const urdf::ConstLinkPtr root = robot_model.getRoot(); // constructs the optional inertia RigidBodyInertia inert(0); if (root->inertial) inert = toKdl(root->inertial); // constructs the kdl joint Joint jnt = Joint(fake_root_fixed_joint_name, Joint::None); // construct the kdl segment Segment sgm(root->name, jnt, Frame::Identity(), inert); // add segment to tree tree.addSegment(sgm, fake_root_name); } else { tree = Tree(robot_model.getRoot()->name); // warn if root link has inertia. KDL does not support this if (robot_model.getRoot()->inertial) std::cerr << "The root link " << robot_model.getRoot()->name << " has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." << std::endl; } // add all children for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++) if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) return false; return true; }