C++ urdf::Model类代码示例

bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
	// Reset root mass to zero and give it the proper name
	mBodies[0].mMass = 0;

	// Set gravity to ROS standards
	gravity << 0, 0, -9.81;

	std::vector<boost::shared_ptr<urdf::Link> > links;
	model.getLinks(links);

	boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
	boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);

	if(oldRoot == newRoot || !newRoot)
		process(*oldRoot, 0, Math::SpatialTransform());
	else
		processReverse(*newRoot, 0, 0, Math::SpatialTransform());

	// Rename the root body to our URDF root
	mBodyNameMap.erase("ROOT");
	mBodyNameMap[root] = 0;

	return true;
}

  bool getChainInfoFromRobotModel(urdf::Model &robot_model,
                                  const std::string &root_name,
                                  const std::string &tip_name,
                                  kinematics_msgs::KinematicSolverInfo &chain_info) 
  {
    // get joint maxs and mins
    boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
    boost::shared_ptr<const urdf::Joint> joint;
    while (link && link->name != root_name) 
    {
      joint = robot_model.getJoint(link->parent_joint->name);
      if (!joint) 
      {
        ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
        return false;
      }
      if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) 
      {
        float lower, upper;
        int hasLimits;
        if ( joint->type != urdf::Joint::CONTINUOUS ) 
        {
          lower = joint->limits->lower;
          upper = joint->limits->upper;
          hasLimits = 1;
        } 
        else 
        {
          lower = -M_PI;
          upper = M_PI;
          hasLimits = 0;
        }
        chain_info.joint_names.push_back(joint->name);
        motion_planning_msgs::JointLimits limits;
        limits.joint_name = joint->name;
        limits.has_position_limits = hasLimits;
        limits.min_position = lower;
        limits.max_position = upper;
        chain_info.limits.push_back(limits);
      }
      link = robot_model.getLink(link->getParent()->name);
    }
    link = robot_model.getLink(tip_name);
    if(link)
      chain_info.link_names.push_back(tip_name);    

    std::reverse(chain_info.limits.begin(),chain_info.limits.end());
    std::reverse(chain_info.joint_names.begin(),chain_info.joint_names.end());

    return true;
  }

void makeJointMarker(std::string jointName)
{
	boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(jointName);

	// The marker must be created in the parent frame so you don't get feedback when you move it
	visualization_msgs::InteractiveMarker marker;

	marker.scale = .125;
	marker.name = jointName;
	marker.header.frame_id = targetJoint->parent_link_name;

	geometry_msgs::Pose controlPose = hubo_motion_ros::toPose( targetJoint->parent_to_joint_origin_transform);
	marker.pose = controlPose;

	visualization_msgs::InteractiveMarkerControl control;

	Eigen::Quaternionf jointAxis;
	Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
	jointAxis.setFromTwoVectors(Eigen::Vector3f::UnitX(), axisVector);

	control.orientation.w = jointAxis.w();
	control.orientation.x = jointAxis.x();
	control.orientation.y = jointAxis.y();
	control.orientation.z = jointAxis.z();

	control.always_visible = true;
	control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
	control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;

	marker.controls.push_back(control);

	gIntServer->insert(marker);
	gIntServer->setCallback(marker.name, &processFeedback);
}

 bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &root_name, std::string &tip_name, std::string &xml_string)
 {
   std::string urdf_xml,full_urdf_xml;
   node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
   node_handle.searchParam(urdf_xml,full_urdf_xml);
   TiXmlDocument xml;
   ROS_DEBUG("Reading xml file from parameter server\n");
   std::string result;
   if (node_handle.getParam(full_urdf_xml, result))
     xml.Parse(result.c_str());
   else
   {
     ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
     return false;
   }
   xml_string = result;
   TiXmlElement *root_element = xml.RootElement();
   TiXmlElement *root = xml.FirstChildElement("robot");
   if (!root || !root_element)
   {
     ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
     exit(1);
   }
   robot_model.initXml(root);
   if (!node_handle.getParam("root_name", root_name)){
     ROS_FATAL("No root name found on parameter server");
     return false;
   }
   if (!node_handle.getParam("tip_name", tip_name)){
     ROS_FATAL("No tip name found on parameter server");
     return false;
   }
   return true;
 }

/* ------------------------ KinematicModel ------------------------ */
planning_models::KinematicModel::KinematicModel(const urdf::Model &model, 
                                                const std::vector<GroupConfig>& group_configs,
                                                const std::vector<MultiDofConfig>& multi_dof_configs)
{    
  model_name_ = model.getName();
  if (model.getRoot())
  {
    const urdf::Link *root = model.getRoot().get();
    root_ = buildRecursive(NULL, root, multi_dof_configs);
    buildGroups(group_configs);
  }
  else
  {
    root_ = NULL;
    ROS_WARN("No root link found");
  }
}

bool FkLookup::addRoot(const urdf::Model &model, const std::string &root){
    boost::shared_ptr<KDL::Tree> tree;
    tree_map::iterator it = roots.find(root);
    if(it != roots.end()){
	ROS_WARN_STREAM("Link " << root << " already processed");
	return false;
    }
    if(!model.getLink(root)){
	ROS_ERROR_STREAM("Model does not include link '" << root << "'");
	return false;
    }
    tree.reset(new KDL::Tree());
    kdl_parser::treeFromUrdfModel(model, *tree);
    roots.insert(std::make_pair(root,tree));
    
    crawl_and_add_links(model.getLink(root)->child_links,root,tips);
    
    return !tips.empty();
}

SolverInfoProcessor::SolverInfoProcessor(const urdf::Model &robot_model,
	const std::string &tip_name,
	const std::string &root_name)
{
	// setup the IK solver information which contains joint names, joint limits and so on
	boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
	while (link) {
		// check if we have already reached the final link
		if (link->name == root_name) break;

		// if we have reached the last joint the root frame is not in the chain
		// then we cannot build the chain
		if (!link->parent_joint) {
			ROS_ERROR("The provided root link is not in the chain");
			ROS_ASSERT(false);
		}

		// process the joint
		boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
		if (!joint) {
			ROS_ERROR("Could not find joint: %s", link->parent_joint->name.c_str());
			ROS_ASSERT(false);
		}

		// add the joint to the chain
		if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
			ROS_DEBUG("Joint axis: %f, %f, %f", joint->axis.x, joint->axis.y, joint->axis.z);

			addJointToChainInfo(link->parent_joint, _solver_info);
		}

		link = robot_model.getLink(link->getParent()->name);
        }

	_solver_info.link_names.push_back(tip_name);

	// We expect order from root to tip, so reverse the order
	std::reverse(_solver_info.limits.begin(), _solver_info.limits.end());
	std::reverse(_solver_info.joint_names.begin(), _solver_info.joint_names.end());
	std::reverse(_solver_info.link_names.begin(), _solver_info.link_names.end());
}

  /// /////////////////////////////////////////////////////////////////////////////
  /// @brief load URDF model description from string and create search operations data structures
  void URDFRenderer::loadURDFModel
    (urdf::Model &model)
  {
    typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
    V_Link links;
    model.getLinks(links);

    V_Link::iterator it = links.begin();
    V_Link::iterator end = links.end();

    for (; it != end; ++it)
      process_link (*it);
  }

bool Kinematics::readJoints(urdf::Model &robot_model) {
    num_joints = 0;
    // get joint maxs and mins
    boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
    boost::shared_ptr<const urdf::Joint> joint;

    while (link && link->name != root_name) {
        joint = robot_model.getJoint(link->parent_joint->name);
        if (!joint) {
            ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
            return false;
        }
        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
            ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
            num_joints++;
        }
        link = robot_model.getLink(link->getParent()->name);
    }

    joint_min.resize(num_joints);
    joint_max.resize(num_joints);
    info.joint_names.resize(num_joints);
    info.link_names.resize(num_joints);
    info.limits.resize(num_joints);

    link = robot_model.getLink(tip_name);
    unsigned int i = 0;
    while (link && i < num_joints) {
        joint = robot_model.getJoint(link->parent_joint->name);
        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
            ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );

            float lower, upper;
            int hasLimits;
            if ( joint->type != urdf::Joint::CONTINUOUS ) {
                lower = joint->limits->lower;
                upper = joint->limits->upper;
                hasLimits = 1;
            } else {
                lower = -M_PI;
                upper = M_PI;
                hasLimits = 0;
            }
            int index = num_joints - i -1;

            joint_min.data[index] = lower;
            joint_max.data[index] = upper;
            info.joint_names[index] = joint->name;
            info.link_names[index] = link->name;
            info.limits[index].joint_name = joint->name;
            info.limits[index].has_position_limits = hasLimits;
            info.limits[index].min_position = lower;
            info.limits[index].max_position = upper;
            i++;
        }
        link = robot_model.getLink(link->getParent()->name);
    }
    return true;
}

bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
	// Display debug information
#if DISPLAY_DEBUG_INFO
	ROS_ERROR("Initialising RBDL model of '%s' with root link '%s'...", model.getName().c_str(), root.c_str());
#endif
	
	// Save the root link name
	m_nameURDFRoot = model.getRoot()->name;
	m_indexURDFRoot = (unsigned int) -1;
	
	// Reset root mass to zero and give it the proper name
	mBodies[0].mMass = 0;

	// Set gravity to ROS standards
	gravity << 0, 0, -9.81;

	std::vector<boost::shared_ptr<urdf::Link> > links;
	model.getLinks(links);

	boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
	boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);

	if(oldRoot == newRoot || !newRoot)
		process(*oldRoot, 0, Math::SpatialTransform());
	else
		processReverse(*newRoot, 0, NULL, Math::SpatialTransform());

	// Rename the root body to our URDF root
	mBodyNameMap.erase("ROOT");
	mBodyNameMap[root] = 0;

	// Display debug information
#if DISPLAY_DEBUG_INFO
	ROS_INFO("URDF root link is '%s' and was detected at RBDL body index %u", m_nameURDFRoot.c_str(), m_indexURDFRoot);
#endif

	return true;
}

bool kdl_urdf_tools::initialize_kinematics_from_urdf(
    const std::string &robot_description,
    const std::string &root_link,
    const std::string &tip_link,
    unsigned int &n_dof,
    KDL::Chain &kdl_chain,
    KDL::Tree &kdl_tree,
    urdf::Model &urdf_model)
{
  if(robot_description.length() == 0) {
    ROS_ERROR("URDF string is empty.");
    return false;
  }

  // Construct an URDF model from the xml string
  urdf_model.initString(robot_description);

  // Get a KDL tree from the robot URDF
  if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
    ROS_ERROR("Failed to construct kdl tree");
    return false;
  }

  // Populate the KDL chain
  if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
  {
    ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
    ROS_ERROR_STREAM("  "<<root_link<<" --> "<<tip_link);
    ROS_ERROR_STREAM("  Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
    ROS_ERROR_STREAM("  Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
    ROS_ERROR_STREAM("  The segments are:");

    KDL::SegmentMap segment_map = kdl_tree.getSegments();
    KDL::SegmentMap::iterator it;

    for( it=segment_map.begin();
        it != segment_map.end();
        it++ )
    {
      ROS_ERROR_STREAM( "    "<<(*it).first);
    }

    return false;
  }

  // Store the number of degrees of freedom of the chain
  n_dof = kdl_chain.getNrOfJoints();

  return true;
}

bool get_model(urdf::Model& robot)
{
  ros::NodeHandle nh;
  std::string file;
  nh.getParam("urdf_file_path", file);
  TiXmlDocument robot_model_xml;
  robot_model_xml.LoadFile(file);
  TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
  if (!robot_xml){
    std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
    return false;
  }

  if (!robot.initXml(robot_xml)){
    std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
    return false;
  }
  return true;
}

  /**
   * TaskSpacePosition annotations.
   *
   * @return UIMA error id. UIMA_ERR_NONE on success.
   */
  uima::TyErrorId annotateTaskSpace(
    uima::CAS& cas,
    const urdf::Model& model
  ) {
    uima::FSIndexRepository& index = cas.getIndexRepository();
    uima::FeatureStructure ts;

    boost::shared_ptr<urdf::Link> link;
    std::vector< boost::shared_ptr<urdf::Link> > links;
    model.getLinks(links);

    for (std::size_t i = 0, size = links.size(); i < size; i++) {
      link = links[i];
      ts = cas.createFS(TaskSpacePosition);
      ts.setFSValue(tsXyzFtr,
        utils::toDoubleArrayFS(cas, MongoUrdf::getPosition(link)));
      ts.setFSValue(tsRpyFtr,
        utils::toDoubleArrayFS(cas, MongoUrdf::getRotation(link)));
      index.addFS(ts);
    }

    return UIMA_ERR_NONE;
  }

bool Pod::init(hardware_interface::EffortJointInterface* hw, urdf::Model urdf) {
  std::string joint_name;

  nh_.param("command_timeout", command_timeout_, command_timeout_);

  if (!nh_.getParam("joint", joint_name)) {
    ROS_ERROR("No joint given (namespace: %s)", nh_.getNamespace().c_str());
    return false;
  }

  joint_ = hw->getHandle(joint_name);
  boost::shared_ptr<const urdf::Joint> joint_urdf =  urdf.getJoint(joint_name);
  if (!joint_urdf) {
    ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
    return false;
  }

  if (!pid_controller_.init(ros::NodeHandle(nh_, "pid")))
    return false;

  controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(nh_, "state", 1));

  command_sub_ = nh_.subscribe<walrus_pod_controller::PodCommand>("command", 1, &Pod::setCommandCallback, this);
}

bool TreeKinematics::readJoints(urdf::Model &robot_model,
                                KDL::Tree &kdl_tree,
                                std::string &tree_root_name,
                                unsigned int &nr_of_jnts,
                                KDL::JntArray &joint_min,
                                KDL::JntArray &joint_max,
                                KDL::JntArray &joint_vel_max)
{
  KDL::SegmentMap segmentMap;
  segmentMap = kdl_tree.getSegments();
  tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
  nr_of_jnts = kdl_tree.getNrOfJoints();
  ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
  joint_min.resize(nr_of_jnts);
  joint_max.resize(nr_of_jnts);
  joint_vel_max.resize(nr_of_jnts);
  info_.joint_names.resize(nr_of_jnts);
  info_.limits.resize(nr_of_jnts);

  // The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
  // the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
  // urdf::Joint::FIXED
  ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
  boost::shared_ptr<const urdf::Joint> joint;
  for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
  {
    if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
    {
      // check, if joint can be found in the URDF model of the robot
      joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
      if (!joint)
      {
        ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
        return false;
      }
      // extract joint information
      if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
      {
        ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
        double lower = 0.0, upper = 0.0, vel_limit = 0.0;
        unsigned int has_pos_limits = 0, has_vel_limits = 0;

        if ( joint->type != urdf::Joint::CONTINUOUS )
        {
          ROS_DEBUG("joint is not continuous.");
          lower = joint->limits->lower;
          upper = joint->limits->upper;
          has_pos_limits = 1;
          if (joint->limits->velocity)
          {
            has_vel_limits = 1;
            vel_limit = joint->limits->velocity;
            ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
          }
          else
          {
            has_vel_limits = 0;
            vel_limit = 0.0;
            ROS_DEBUG("joint has no velocity limit.");
          }
        }
        else
        {
          ROS_DEBUG("joint is continuous.");
          lower = -M_PI;
          upper = M_PI;
          has_pos_limits = 0;
          if(joint->limits && joint->limits->velocity)
          {
            has_vel_limits = 1;
            vel_limit = joint->limits->velocity;
            ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
          }
          else
          {
            has_vel_limits = 0;
            vel_limit = 0.0;
            ROS_DEBUG("joint has no velocity limit.");
          }
        }

        joint_min(seg_it->second.q_nr) = lower;
        joint_max(seg_it->second.q_nr) = upper;
        joint_vel_max(seg_it->second.q_nr) = vel_limit;
        ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);

        info_.joint_names[seg_it->second.q_nr] = joint->name;
        info_.limits[seg_it->second.q_nr].joint_name = joint->name;
        info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
        info_.limits[seg_it->second.q_nr].min_position = lower;
        info_.limits[seg_it->second.q_nr].max_position = upper;
        info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
        info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
      }
    }
  }
  return true;
}