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;
}
Example #2
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;
}
Example #3
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;
}
Example #4
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);

}
Example #5
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() );
}