Esempio n. 1
1
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;
}
Esempio n. 2
0
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;
}