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;
}