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
void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
  for (TiXmlElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml; c_xml = c_xml->NextSiblingElement("disable_collisions"))
  {
    const char *link1 = c_xml->Attribute("link1");
    const char *link2 = c_xml->Attribute("link2");
    if (!link1 || !link2)
    {
      logError("A pair of links needs to be specified to disable collisions");
      continue;
    }
    DisabledCollision dc;
    dc.link1_ = boost::trim_copy(std::string(link1));
    dc.link2_ = boost::trim_copy(std::string(link2));
    if (!urdf_model.getLink(dc.link1_))
    {
      logError("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
      continue;
    }
    if (!urdf_model.getLink(dc.link2_))
    {
      logError("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
      continue;
    }
    const char *reason = c_xml->Attribute("reason");
    if (reason)
      dc.reason_ = std::string(reason);
    disabled_collisions_.push_back(dc);
  }
}
bool URDFtoRSG::rsgFromUrdfModel(const urdf::ModelInterface& robot_model)
{
  std::cout << "Found a robot with root :" << robot_model.getRoot()->name << std::endl;
  //  add all children
  for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++)
    if (!addChildrenToRSG(robot_model.getRoot()->child_links[i], wm->getRootNodeId())) return false;

  return true;
}
Esempio n. 4
0
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
{
    tree = Tree(robot_model.getRoot()->name);

    // warn if root link has inertia. KDL does not support this
    if (robot_model.getRoot()->inertial)
        std::cout<<"The root link "<<robot_model.getRoot()->name.c_str()<<" 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;
    // ROS_WARN("The root link %s 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.", robot_model.getRoot()->name.c_str());

    //  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;
}
Esempio n. 5
0
void srdf::Model::loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{  
  for (TiXmlElement* c_xml = robot_xml->FirstChildElement("passive_joint"); c_xml; c_xml = c_xml->NextSiblingElement("passive_joint"))
  {
    const char *name = c_xml->Attribute("name");
    if (!name)
    {
      logError("No name specified for passive joint. Ignoring.");
      continue;
    }
    PassiveJoint pj;
    pj.name_ = boost::trim_copy(std::string(name));

    // see if a virtual joint was marked as passive
    bool vjoint = false;
    for (std::size_t i = 0 ; !vjoint && i < virtual_joints_.size() ; ++i)
      if (virtual_joints_[i].name_ == pj.name_)
        vjoint = true;
    
    if (!vjoint && !urdf_model.getJoint(pj.name_))
    {
      logError("Joint '%s' marked as passive is not known to the URDF. Ignoring.", name);
      continue;
    }
    passive_joints_.push_back(pj);
  }
}
Esempio n. 6
0
void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
  for (TiXmlElement* eef_xml = robot_xml->FirstChildElement("end_effector"); eef_xml; eef_xml = eef_xml->NextSiblingElement("end_effector"))
  {
    const char *ename = eef_xml->Attribute("name");
    const char *gname = eef_xml->Attribute("group");
    const char *parent = eef_xml->Attribute("parent_link");
    const char *parent_group = eef_xml->Attribute("parent_group");
    if (!ename)
    {
      logError("Name of end effector is not specified");
      continue;
    }
    if (!gname)
    {
      logError("Group not specified for end effector '%s'", ename);
      continue;
    }
    EndEffector e;
    e.name_ = std::string(ename); boost::trim(e.name_);
    e.component_group_ = std::string(gname); boost::trim(e.component_group_);
    bool found = false;
    for (std::size_t k = 0 ; k < groups_.size() ; ++k)
      if (groups_[k].name_ == e.component_group_)
      {
        found = true;
        break;
      }
    if (!found)
    {
      logError("End effector '%s' specified for group '%s', but that group is not known", ename, gname);
      continue;
    }
    if (!parent)
    {
      logError("Parent link not specified for end effector '%s'", ename);
      continue;
    }
    e.parent_link_ = std::string(parent); boost::trim(e.parent_link_);
    if (!urdf_model.getLink(e.parent_link_))
    {
      logError("Link '%s' specified as parent for end effector '%s' is not known to the URDF", parent, ename);
      continue;
    }
    if (parent_group)
    {
      e.parent_group_ = std::string(parent_group); boost::trim(e.parent_group_);
    }
    end_effectors_.push_back(e);
  }
}
Esempio n. 7
0
void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
  for (TiXmlElement* vj_xml = robot_xml->FirstChildElement("virtual_joint"); vj_xml; vj_xml = vj_xml->NextSiblingElement("virtual_joint"))
  {
    const char *jname = vj_xml->Attribute("name");
    const char *child = vj_xml->Attribute("child_link");
    const char *parent = vj_xml->Attribute("parent_frame");
    const char *type = vj_xml->Attribute("type");
    if (!jname)
    {
      logError("Name of virtual joint is not specified");
      continue;
    }
    if (!child)
    {
      logError("Child link of virtual joint is not specified");
      continue;
    }
    if (!urdf_model.getLink(boost::trim_copy(std::string(child))))
    {
      logError("Virtual joint does not attach to a link on the robot (link '%s' is not known)", child);
      continue;
    }
    if (!parent)
    {
      logError("Parent frame of virtual joint is not specified");
      continue;
    }
    if (!type)
    {
      logError("Type of virtual joint is not specified");
      continue;
    }
    VirtualJoint vj;
    vj.type_ = std::string(type); boost::trim(vj.type_);
    std::transform(vj.type_.begin(), vj.type_.end(), vj.type_.begin(), ::tolower);
    if (vj.type_ != "planar" && vj.type_ != "floating" && vj.type_ != "fixed")
    {
      logError("Unknown type of joint: '%s'. Assuming 'fixed' instead. Other known types are 'planar' and 'floating'.", type);
      vj.type_ = "fixed";
    }
    vj.name_ = std::string(jname); boost::trim(vj.name_);        
    vj.child_link_ = std::string(child); boost::trim(vj.child_link_);
    vj.parent_frame_ = std::string(parent); boost::trim(vj.parent_frame_);
    virtual_joints_.push_back(vj);
  }
}
Esempio n. 8
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;
}
Esempio n. 9
0
void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
{
  // clear out any data (properties, shapes, etc) from a previously loaded robot.
  clear();

  // the root link is discovered below.  Set to NULL until found.
  root_link_ = NULL;

  // Create properties for each link.
  {
    typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
    M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
    M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
    for( ; link_it != link_end; ++link_it )
    {
      const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
      std::string parent_joint_name;

      if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
      {
        parent_joint_name = urdf_link->parent_joint->name;
      }

      RobotLink* link = new RobotLink(this, urdf_link, parent_joint_name,
                                   visual, collision, link_pointclouds_);

      if (urdf_link == urdf.getRoot())
      {
        root_link_ = link;
      }

      links_[urdf_link->name] = link;
    }
  }

  // Create properties for each joint.
  {
    typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
    M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
    M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
    for( ; joint_it != joint_end; ++joint_it )
    {
      const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
      RobotJoint* joint = new RobotJoint(this, urdf_joint);

      joints_[urdf_joint->name] = joint;
    }
  }

  link_pointclouds_.syncToDevice(); // after all links have been created, we sync them to the GPU

  // finally create a KDL representation of the kinematic tree:
  if (!kdl_parser::treeFromUrdfModel(urdf, tree)){
    LOGGING_ERROR_C(RobotLog, Robot,
                    "Failed to extract kdl tree from xml robot description!" << endl);
    exit(-1);
  }

  LOGGING_INFO_C(RobotLog, Robot,
                 "Constructed KDL tree has " << tree.getNrOfJoints() << " Joints and "
                 << tree.getNrOfSegments() << " segments." << endl);

}
Esempio n. 10
0
void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
{
  link_tree_->hide(); // hide until loaded
  robot_loaded_ = false;

  // clear out any data (properties, shapes, etc) from a previously loaded robot.
  clear();

  // the root link is discovered below.  Set to NULL until found.
  root_link_ = NULL;

  // Create properties for each link.
  // Properties are not added to display until changedLinkTreeStyle() is called (below).
  {
    typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
    M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
    M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
    for( ; link_it != link_end; ++link_it )
    {
      const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
      std::string parent_joint_name;

      if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
      {
        parent_joint_name = urdf_link->parent_joint->name;
      }
      
      RobotLink* link = link_factory_->createLink( this,
                                                   urdf_link,
                                                   parent_joint_name,
                                                   visual,
                                                   collision );

      if (urdf_link == urdf.getRoot())
      {
        root_link_ = link;
      }

      links_[urdf_link->name] = link;

      link->setRobotAlpha( alpha_ );
    }
  }

  // Create properties for each joint.
  // Properties are not added to display until changedLinkTreeStyle() is called (below).
  {
    typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
    M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
    M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
    for( ; joint_it != joint_end; ++joint_it )
    {
      const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
      RobotJoint* joint = link_factory_->createJoint( this, urdf_joint );

      joints_[urdf_joint->name] = joint;

      joint->setRobotAlpha( alpha_ );
    }
  }

  // robot is now loaded
  robot_loaded_ = true;
  link_tree_->show();

  // set the link tree style and add link/joint properties to rviz pane.
  setLinkTreeStyle(LinkTreeStyle(link_tree_style_->getOptionInt()));
  changedLinkTreeStyle();

  // at startup the link tree is collapsed since it is large and not often needed.
  link_tree_->collapse();

  setVisualVisible( isVisualVisible() );
  setCollisionVisible( isCollisionVisible() );
}
Esempio n. 11
0
bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model)
{
    robot_model.clear();
    robot_model.name_ = robot_name;

    //Add all links
    KDL::SegmentMap::iterator seg;
    KDL::SegmentMap segs;
    KDL::SegmentMap::const_iterator root_seg;
    root_seg = tree.getRootSegment();
    segs = tree.getSegments();
    for( seg = segs.begin(); seg != segs.end(); seg++ ) {
        if (robot_model.getLink(seg->first))
        {
            std::cerr << "[ERR] link " << seg->first << " is not unique." << std::endl;
            robot_model.clear();
            return false;
        }
        else
        {
            urdf::LinkPtr link;
            resetPtr(link, new urdf::Link);

            //add name
            link->name = seg->first;

            //insert link
            robot_model.links_.insert(make_pair(seg->first,link));
            std::cerr << "[DEBUG] successfully added a new link " << link->name << std::endl;
        }

        //inserting joint
        //The fake root segment has no joint to add
        if( seg->first != root_seg->first ) {
            KDL::Joint jnt;
            jnt = GetTreeElementSegment(seg->second).getJoint();
            if (robot_model.getJoint(jnt.getName()))
            {
                std::cerr << "[ERR] joint " <<  jnt.getName() << " is not unique." << std::endl;
                robot_model.clear();
                return false;
            }
            else
            {
                urdf::JointPtr joint;
                urdf::LinkPtr link = robot_model.links_[seg->first];
                //This variable will be set by toUrdf
                KDL::Frame H_new_old_successor;
                KDL::Frame H_new_old_predecessor = getH_new_old(GetTreeElementSegment(GetTreeElementParent(seg->second)->second));
                urdf::resetPtr(joint, new urdf::Joint());

                //convert joint
                *joint = toUrdf(jnt, GetTreeElementSegment(seg->second).getFrameToTip(),H_new_old_predecessor,H_new_old_successor);

                //insert parent
                joint->parent_link_name = GetTreeElementParent(seg->second)->first;

                //insert child
                joint->child_link_name = seg->first;

                //insert joint
                robot_model.joints_.insert(make_pair(seg->first,joint));
                std::cerr << "[DEBUG] successfully added a new joint" << jnt.getName() << std::endl;

                //add inertial, taking in account an eventual change in the link frame
                resetPtr(link->inertial, new urdf::Inertial());
                *(link->inertial) = toUrdf(H_new_old_successor * GetTreeElementSegment(seg->second).getInertia());
            }
        }

    }

    // every link has children links and joints, but no parents, so we create a
    // local convenience data structure for keeping child->parent relations
    std::map<std::string, std::string> parent_link_tree;
    parent_link_tree.clear();

    // building tree: name mapping
    //try
    //{
        robot_model.initTree(parent_link_tree);
    //}
    /*
    catch(ParseError &e)
    {
        logError("Failed to build tree: %s", e.what());
        robot_model.clear();
        return false;
    }*/

    // find the root link
    //try
    //{
        robot_model.initRoot(parent_link_tree);
    //}
    /*
    catch(ParseError &e)
    {
        logError("Failed to find root link: %s", e.what());
        robot_model.reset();
        return false;
    }
    */
    return true;
}
Esempio n. 12
0
void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
  for (TiXmlElement* group_xml = robot_xml->FirstChildElement("group"); group_xml; group_xml = group_xml->NextSiblingElement("group"))
  {
    const char *gname = group_xml->Attribute("name");
    if (!gname)
    {
      logError("Group name not specified");
      continue;
    }
    Group g;
    g.name_ = std::string(gname); boost::trim(g.name_);
    
    // get the links in the groups
    for (TiXmlElement* link_xml = group_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
    {
      const char *lname = link_xml->Attribute("name");
      if (!lname)
      {
        logError("Link name not specified");
        continue;
      }
      std::string lname_str = boost::trim_copy(std::string(lname));
      if (!urdf_model.getLink(lname_str))
      {
        logError("Link '%s' declared as part of group '%s' is not known to the URDF", lname, gname);
        continue;
      }
      g.links_.push_back(lname_str);
    }
    
    // get the joints in the groups
    for (TiXmlElement* joint_xml = group_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
    {
      const char *jname = joint_xml->Attribute("name");
      if (!jname)
      {
        logError("Joint name not specified");
        continue;
      }
      std::string jname_str = boost::trim_copy(std::string(jname));
      if (!urdf_model.getJoint(jname_str))
      {
        bool missing = true;
        for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
          if (virtual_joints_[k].name_ == jname_str)
          {
            missing = false;
            break;
          }
        if (missing)
        {
          logError("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname);
          continue;
        }
      }
      g.joints_.push_back(jname_str);
    }
    
    // get the chains in the groups
    for (TiXmlElement* chain_xml = group_xml->FirstChildElement("chain"); chain_xml; chain_xml = chain_xml->NextSiblingElement("chain"))
    {
      const char *base = chain_xml->Attribute("base_link");
      const char *tip = chain_xml->Attribute("tip_link");
      if (!base)
      {
        logError("Base link name not specified for chain");
        continue;
      }
      if (!tip)
      {
        logError("Tip link name not specified for chain");
        continue;
      }
      std::string base_str = boost::trim_copy(std::string(base));
      std::string tip_str = boost::trim_copy(std::string(tip));
      if (!urdf_model.getLink(base_str))
      {
        logError("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", base, gname);
        continue;
      }
      if (!urdf_model.getLink(tip_str))
      {
        logError("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", tip, gname);
        continue;
      }
      bool found = false;
      boost::shared_ptr<const urdf::Link> l = urdf_model.getLink(tip_str);
      std::set<std::string> seen;
      while (!found && l)
      {
        seen.insert(l->name);
        if (l->name == base_str)
          found = true;
        else
          l = l->getParent();
      }
      if (!found)
      {
        l = urdf_model.getLink(base_str);
        while (!found && l)
        {
          if (seen.find(l->name) != seen.end())
            found = true;
          else
            l = l->getParent();
        }
      }
      if (found)
        g.chains_.push_back(std::make_pair(base_str, tip_str));
      else
        logError("Links '%s' and '%s' do not form a chain. Not included in group '%s'", base, tip, gname);
    }
    
    // get the subgroups in the groups
    for (TiXmlElement* subg_xml = group_xml->FirstChildElement("group"); subg_xml; subg_xml = subg_xml->NextSiblingElement("group"))
    {
      const char *sub = subg_xml->Attribute("name");
      if (!sub)
      {
        logError("Group name not specified when included as subgroup");
        continue;
      }
      g.subgroups_.push_back(boost::trim_copy(std::string(sub)));
    }
    if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty())
      logWarn("Group '%s' is empty.", gname);
    groups_.push_back(g);
  }
  
  // check the subgroups
  std::set<std::string> known_groups;
  bool update = true;
  while (update)
  {
    update = false;
    for (std::size_t i = 0 ; i < groups_.size() ; ++i)
    {
      if (known_groups.find(groups_[i].name_) != known_groups.end())
        continue;
      if (groups_[i].subgroups_.empty())
      {
        known_groups.insert(groups_[i].name_);
        update = true;
      }
      else
      {
        bool ok = true;
        for (std::size_t j = 0 ; ok && j < groups_[i].subgroups_.size() ; ++j)
          if (known_groups.find(groups_[i].subgroups_[j]) == known_groups.end())
            ok = false;
        if (ok)
        {
          known_groups.insert(groups_[i].name_);
          update = true;
        }
      }
    }
  }
  
  // if there are erroneous groups, keep only the valid ones
  if (known_groups.size() != groups_.size())
  {
    std::vector<Group> correct;
    for (std::size_t i = 0 ; i < groups_.size() ; ++i)
      if (known_groups.find(groups_[i].name_) != known_groups.end())
        correct.push_back(groups_[i]);
      else
        logError("Group '%s' has unsatisfied subgroups", groups_[i].name_.c_str());
    groups_.swap(correct);
  }
}
Esempio n. 13
0
void srdf::Model::loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
  for (TiXmlElement* cslink_xml = robot_xml->FirstChildElement("link_sphere_approximation"); cslink_xml; cslink_xml = cslink_xml->NextSiblingElement("link_sphere_approximation"))
  {
    int non_0_radius_sphere_cnt = 0;
    const char *link_name = cslink_xml->Attribute("link");
    if (!link_name)
    {
      logError("Name of link is not specified in link_collision_spheres");
      continue;
    }
    
    LinkSpheres link_spheres;
    link_spheres.link_ = boost::trim_copy(std::string(link_name));
    if (!urdf_model.getLink(link_spheres.link_))
    {
      logError("Link '%s' is not known to URDF.", link_name);
      continue;
    }
    

    // get the spheres for this link
    int cnt = 0;
    for (TiXmlElement* sphere_xml = cslink_xml->FirstChildElement("sphere"); sphere_xml; sphere_xml = sphere_xml->NextSiblingElement("sphere"), cnt++)
    {
      const char *s_center = sphere_xml->Attribute("center");
      const char *s_r = sphere_xml->Attribute("radius");
      if (!s_center || !s_r)
      {
        logError("Link collision sphere %d for link '%s' does not have both center and radius.", cnt, link_name);
        continue;
      }

      Sphere sphere;
      try
      {
        std::stringstream center(s_center);
        center.exceptions(std::stringstream::failbit | std::stringstream::badbit);
        center >> sphere.center_x_ >> sphere.center_y_ >> sphere.center_z_;
        sphere.radius_ = boost::lexical_cast<double>(s_r);
      }
      catch (std::stringstream::failure &e)
      {
        logError("Link collision sphere %d for link '%s' has bad center attribute value.", cnt, link_name);
        continue;
      }
      catch (boost::bad_lexical_cast &e)
      {
        logError("Link collision sphere %d for link '%s' has bad radius attribute value.", cnt, link_name);
        continue;
      }

      // ignore radius==0 spheres unless there is only 1 of them
      //
      // NOTE:
      //  - If a link has no sphere_approximation then one will be generated
      //     automatically containing a single sphere which encloses the entire
      //     collision geometry.  Internally this is represented by not having
      //     a link_sphere_approximations_ entry for this link.
      //  - If a link has only spheres with radius 0 then it will not be
      //     considered for collision detection.  In this case the internal
      //     representation is a single radius=0 sphere.
      //  - If a link has at least one sphere with radius>0 then those spheres
      //     are used.  Any radius=0 spheres are eliminated.
      if (sphere.radius_ > std::numeric_limits<double>::epsilon())
      {
        if (non_0_radius_sphere_cnt == 0)
          link_spheres.spheres_.clear();
        link_spheres.spheres_.push_back(sphere);
        non_0_radius_sphere_cnt++;
      }
      else if (non_0_radius_sphere_cnt == 0)
      {
        sphere.center_x_ = 0.0;
        sphere.center_y_ = 0.0;
        sphere.center_z_ = 0.0;
        sphere.radius_ = 0.0;
        link_spheres.spheres_.clear();
        link_spheres.spheres_.push_back(sphere);
      }
    }

    if (!link_spheres.spheres_.empty())
      link_sphere_approximations_.push_back(link_spheres);
  }
}
Esempio n. 14
0
void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
{
  for (TiXmlElement* gstate_xml = robot_xml->FirstChildElement("group_state"); gstate_xml; gstate_xml = gstate_xml->NextSiblingElement("group_state"))
  {
    const char *sname = gstate_xml->Attribute("name");
    const char *gname = gstate_xml->Attribute("group");
    if (!sname)
    {
      logError("Name of group state is not specified");
      continue;
    }
    if (!gname)
    {
      logError("Name of group for state '%s' is not specified", sname);
      continue;
    }
    
    GroupState gs;
    gs.name_ = boost::trim_copy(std::string(sname));
    gs.group_ = boost::trim_copy(std::string(gname));
    
    bool found = false;
    for (std::size_t k = 0 ; k < groups_.size() ; ++k)
      if (groups_[k].name_ == gs.group_)
      {
        found = true;
        break;
      }
    if (!found)
    {
      logError("Group state '%s' specified for group '%s', but that group is not known", sname, gname);
      continue;
    }
    
    // get the joint values in the group state
    for (TiXmlElement* joint_xml = gstate_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
    {
      const char *jname = joint_xml->Attribute("name");
      const char *jval = joint_xml->Attribute("value");
      if (!jname)
      {
        logError("Joint name not specified in group state '%s'", sname);
        continue;
      }
      if (!jval)
      {
        logError("Joint name not specified for joint '%s' in group state '%s'", jname, sname);
        continue;
      }
      std::string jname_str = boost::trim_copy(std::string(jname));
      if (!urdf_model.getJoint(jname_str))
      {
        bool missing = true;
        for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
          if (virtual_joints_[k].name_ == jname_str)
          {
            missing = false;
            break;
          }
        if (missing)
        {
          logError("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, sname);
          continue;
        }
      }
      try
      {
        std::string jval_str = std::string(jval);
        std::stringstream ss(jval_str);
        while (ss.good() && !ss.eof())
        {
          std::string val; ss >> val >> std::ws;
          gs.joint_values_[jname_str].push_back(boost::lexical_cast<double>(val));
        }
      }
      catch (boost::bad_lexical_cast &e)
      {
        logError("Unable to parse joint value '%s'", jval);
      }
      
      if (gs.joint_values_.empty())
        logError("Unable to parse joint value ('%s') for joint '%s' in group state '%s'", jval, jname, sname);
    }
    group_states_.push_back(gs);
  }
}