C++ Model::getLink方法代码示例

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

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

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