C++ Chain::getSegment方法代码示例

void getKDLChainInfo(kinematics_msgs::KinematicSolverInfo &chain_info)
{
	unsigned int nj = chain.getNrOfJoints();
	unsigned int nl = chain.getNrOfSegments();
	
	ROS_DEBUG("nj: %d", nj);
	ROS_DEBUG("nl: %d", nl);

	//---setting up response

	//joint_names
	for(unsigned int i=0; i<nj; i++)
	{
		ROS_DEBUG("joint_name[%d]: %s", i, chain.getSegment(i).getJoint().getName().c_str());
		chain_info.joint_names.push_back(chain.getSegment(i).getJoint().getName());
	}
	//limits
		//joint limits are only saved in KDL::ChainIkSolverPos_NR_JL iksolverpos -> ik_solve().....but this is not visible here!
/*for(unsigned int i=0; i<nj; i++)
  {
		chain_info.limits[i].joint_name=chain.getSegment(i).getJoint().getName();
		chain_info.limits[i].has_position_limits=
		chain_info.limits[i].min_position=
		chain_info.limits[i].max_position=	
		chain_info.limits[i].has_velocity_limits=
		chain_info.limits[i].max_velocity=
		chain_info.limits[i].has_acceleration_limits=
		chain_info.limits[i].max_acceleration=
	}*/
	//link_names	
	for(unsigned int i=0; i<nl; i++)
	{
		chain_info.link_names.push_back(chain.getSegment(i).getName());
	}
}

void leatherman::printKDLChain(const KDL::Chain &c, std::string text)
{
  ROS_INFO("[%s] # segments: %d  # joints: %d", text.c_str(), c.getNrOfSegments(), c.getNrOfJoints());
  double r,p,y,jnt_pos = 0;
  for(size_t j = 0; j < c.getNrOfSegments(); ++j)
  {
    c.getSegment(j).pose(jnt_pos).M.GetRPY(r,p,y);
    ROS_INFO("[%s] [%2d] segment: %21s  xyz: %0.3f %0.3f %0.3f  rpy: %0.3f %0.3f %0.3f", 
        text.c_str(), int(j),
        c.getSegment(j).getName().c_str(),
        c.getSegment(j).pose(jnt_pos).p.x(),
        c.getSegment(j).pose(jnt_pos).p.y(),
        c.getSegment(j).pose(jnt_pos).p.z(),
        r,p,y);

    c.getSegment(j).getJoint().pose(jnt_pos).M.GetRPY(r,p,y);
    ROS_INFO("[%s]        joint: %21s  xyz: %0.3f %0.3f %0.3f  rpy: %0.3f %0.3f %0.3f  type: %s", 
        text.c_str(),
        c.getSegment(j).getJoint().getName().c_str(),
        c.getSegment(j).getJoint().pose(jnt_pos).p.x(),
        c.getSegment(j).getJoint().pose(jnt_pos).p.y(),
        c.getSegment(j).getJoint().pose(jnt_pos).p.z(),
        r,p,y,
        c.getSegment(j).getJoint().getTypeName().c_str());

  }
}

 void getKDLChainInfo(const KDL::Chain &chain,
                      kinematics_msgs::KinematicSolverInfo &chain_info)
 {
   int i=0; // segment number
   while(i < (int)chain.getNrOfSegments())
   {
     chain_info.link_names.push_back(chain.getSegment(i).getName());
     i++;
   }
 }

int Kinematics::getKDLSegmentIndex(const std::string &name) {
    int i=0; 
    while (i < (int)chain.getNrOfSegments()) {
        if (chain.getSegment(i).getName() == name) {
            return i+1;
        }
        i++;
    }
    return -1;
}

int ExcavaROBArmKinematics::getKDLSegmentIndex(const std::string &name) {
    int i=0; 
    while (i < (int) arm_chain_.getNrOfSegments()) {
        if (arm_chain_.getSegment(i).getName() == name) {
            return i + 1;
        }
        i++;
    }
    return -1;
}

FkLookup::ChainFK::ChainFK(const KDL::Tree& tree, const std::string& root, const std::string& tip):root_name(root),tip_name(tip){
    KDL::Chain chain;
    tree.getChain(root_name, tip_name, chain);
    solver =  new KDL::ChainFkSolverPos_recursive(chain);
    unsigned int  num = chain.getNrOfSegments();
    for(unsigned int i = 0; i < num; ++i){
	const KDL::Joint &joint = chain.getSegment(i).getJoint();
	if (joint.getType() != KDL::Joint::None) joints.insert(std::make_pair(joint.getName(),joints.size()));
    }
    ROS_ASSERT(joints.size() == chain.getNrOfJoints());
}

sensor_msgs::JointState init_message(const KDL::Chain &chain) {
	sensor_msgs::JointState msg;
	for (unsigned int i=0; i < chain.getNrOfSegments(); ++i) {
		KDL::Segment segment = chain.getSegment(i);
		KDL::Joint joint = segment.getJoint();
		if (joint.getType() == KDL::Joint::JointType::None) continue;
		msg.name.push_back(joint.getName());
		msg.position.push_back(0);
	}
	return msg;
}

bool leatherman::getSegmentIndex(const KDL::Chain &c, std::string name, int &index)
{
  for(size_t j = 0; j < c.getNrOfSegments(); ++j)
  {
    if(c.getSegment(j).getName().compare(name) == 0)
    {
      index = j;
      return true;
    }
  }
  return false;
}

    VirtualForcePublisher()
    {
        ros::NodeHandle n_tilde("~");
        ros::NodeHandle n;

        // subscribe to joint state
        joint_state_sub_ = n.subscribe("joint_states", 1, &VirtualForcePublisher::callbackJointState, this);
        wrench_stamped_pub_ = n.advertise<geometry_msgs::WrenchStamped>("wrench", 1);

        // set publish frequency
        double publish_freq;
        n_tilde.param("publish_frequency", publish_freq, 50.0);
        publish_interval_ = ros::Duration(1.0/std::max(publish_freq,1.0));

        //set time constant of low pass filter
        n_tilde.param("time_constant", t_const_, 0.3);
        // set root and tip
        n_tilde.param("root", root, std::string("torso_lift_link"));
        n_tilde.param("tip", tip, std::string("l_gripper_tool_frame"));

        // load robot model
        urdf::Model robot_model;
        robot_model.initParam("robot_description");

        KDL::Tree kdl_tree;
        kdl_parser::treeFromUrdfModel(robot_model, kdl_tree);
        kdl_tree.getChain(root, tip, chain_);
        jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));

        jnt_pos_.resize(chain_.getNrOfJoints());
        jacobian_.resize(chain_.getNrOfJoints());

        for (size_t i=0; i<chain_.getNrOfSegments(); i++) {
            if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) {
                std::cerr << "kdl_chain(" << i << ") " << chain_.getSegment(i).getJoint().getName().c_str() << std::endl;
            }
        }
    }

 int getKDLSegmentIndex(const KDL::Chain &chain, 
                        const std::string &name)
 {
   int i=0; // segment number
   while(i < (int)chain.getNrOfSegments())
   {
     if(chain.getSegment(i).getName() == name)
     {
       return i+1;
     }
     i++;
   }
   return -1;   
 }

bool OrientationConstraint::initialize(const arm_navigation_msgs::OrientationConstraint& constraint,
                const KDL::Chain& chain)
{
  constraint_ = constraint;
  link_id_ = -1;
  for (unsigned int i=0; i<chain.getNrOfSegments(); ++i)
  {
    if (constraint_.link_name == chain.getSegment(i).getName())
    {
      link_id_ = i;
      break;
    }
  }
  if (link_id_ == -1)
  {
    ROS_ERROR("OrientationConstraint: couldn't find link %s in chain.", constraint_.link_name.c_str());
    return false;
  }
  return true;
}

bool fk_solve_all(kinematics_msgs::GetPositionFK::Request &req,
		  kinematics_msgs::GetPositionFK::Response &res )
{
	ROS_INFO("[TESTING]: get_fk_all_service has been called!");

	unsigned int nj = chain.getNrOfJoints();
	unsigned int nl = chain.getNrOfSegments();
	
	ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver

	JntArray conf(nj);
    geometry_msgs::PoseStamped pose;
    tf::Stamped<tf::Pose> tf_pose;

	for(int i = 0; i < nj; i++)
		conf(i) = req.robot_state.joint_state.position[i];
	Frame F_ist;
		res.pose_stamped.resize(nl);
	res.fk_link_names.resize(nl);
	for(int j = 0; j < nl; j++)
	{
		if(fksolver1.JntToCart(conf, F_ist, j+1) >= 0)
		{
		std::cout << "Calculated Position out of Configuration for segment " << j+1 << ":\n";
			std::cout << F_ist <<"\n";
		
			std::cout << "Calculated Position out of Configuration:\n";
			std::cout << F_ist <<"\n";
	
			//TODO: fill out response!!!

		    tf_pose.frame_id_ = "base_link";//root_name_;
		    tf_pose.stamp_ = ros::Time();
		    tf::PoseKDLToTF(F_ist,tf_pose);
		    try
			{
		      //tf_.transformPose(req.header.frame_id,tf_pose,tf_pose);
		    }
		    catch(...)
		    {
		      ROS_ERROR("Could not transform FK pose to frame: %s",req.header.frame_id.c_str());
		      res.error_code.val = res.error_code.FRAME_TRANSFORM_FAILURE;
		      return false;
		    }
		    tf::poseStampedTFToMsg(tf_pose,pose);
		    res.pose_stamped[j] = pose;
		    res.fk_link_names[j] = chain.getSegment(j).getName();
		    res.error_code.val = res.error_code.SUCCESS;
		}
		else
		{
		    ROS_ERROR("Could not compute FK for segment %d", j);
		    res.error_code.val = res.error_code.NO_FK_SOLUTION;
		}


		//TODO: fill out response!!!
		//res.pose_stamped[j]
	}

	return true;
}

    void callbackJointState(const JointStateConstPtr& state)
    {
        std::map<std::string, double> joint_name_position;
        if (state->name.size() != state->position.size()) {
            ROS_ERROR("Robot state publisher received an invalid joint state vector");
            return;
        }

        // determine least recently published joint
        ros::Time last_published = ros::Time::now();
        for (unsigned int i=0; i<state->name.size(); i++) {
            ros::Time t = last_publish_time_[state->name[i]];
            last_published = (t < last_published) ? t : last_published;
        }

        if (state->header.stamp >= last_published + publish_interval_) {
            // get joint positions from state message
            std::map<std::string, double> joint_positions;
            std::map<std::string, double> joint_efforts;
            for (unsigned int i=0; i<state->name.size(); i++) {
                joint_positions.insert(make_pair(state->name[i], state->position[i]));
                joint_efforts.insert(make_pair(state->name[i], state->effort[i]));
            }

            KDL::JntArray  tau;
            KDL::Wrench    F;
            Eigen::Matrix<double,Eigen::Dynamic,6> jac_t;
            Eigen::Matrix<double,6,Eigen::Dynamic> jac_t_pseudo_inv;
            tf::StampedTransform transform;
            KDL::Wrench    F_pub;
            tf::Vector3 tf_force;
            tf::Vector3 tf_torque;

            tau.resize(chain_.getNrOfJoints());

            //getPositions;
            for (size_t i=0, j=0; i<chain_.getNrOfSegments(); i++) {
                if (chain_.getSegment(i).getJoint().getType() == KDL::Joint::None)
                    continue;

                // check
                std::string joint_name = chain_.getSegment(i).getJoint().getName();
                if ( joint_positions.find(joint_name) == joint_positions.end() ) {
                    ROS_ERROR("Joint '%s' is not found in joint state vector", joint_name.c_str());
                }

                // set position
                jnt_pos_(j) = joint_positions[joint_name];
                tau(j) = joint_efforts[joint_name];
                j++;
            }

            jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
            jac_t = jacobian_.data.transpose();
            if ( jacobian_.columns() >= jacobian_.rows() ) {
                jac_t_pseudo_inv =(jac_t.transpose() * jac_t).inverse() *  jac_t.transpose();
            } else {
                jac_t_pseudo_inv =jac_t.transpose() * ( jac_t *  jac_t.transpose() ).inverse();
            }
#if 1
            {
                ROS_INFO("jac_t# jac_t : ");
                Eigen::Matrix<double,6,6> mat_i =  mat_i = jac_t_pseudo_inv * jac_t;
                for (unsigned int i = 0; i < 6; i++) {
                    std::stringstream ss;
                    for (unsigned int j=0; j<6; j++)
                        ss << std::fixed << std::setw(8) << std::setprecision(4) << mat_i(j,i) << " ";
                    ROS_INFO_STREAM(ss.str());
                }
            }
#endif
            // f = - inv(jt) * effort
            for (unsigned int j=0; j<6; j++)
            {
                F(j) = 0;
                for (unsigned int i = 0; i < jnt_pos_.rows(); i++)
                {
                    F(j) += -1 * jac_t_pseudo_inv(j, i) * tau(i);
                }
            }

            //low pass filter
            F += (F_pre_ - F) * exp(-1.0 / t_const_);
            for (unsigned int j=0; j<6; j++) {
                F_pre_(j) = 0;
            }
            F_pre_ += F;

            //tf transformation
            tf::vectorKDLToTF(F.force, tf_force);
            tf::vectorKDLToTF(F.torque, tf_torque);
            try {
                listener_.waitForTransform( tip, root, state->header.stamp, ros::Duration(1.0));
                listener_.lookupTransform( tip, root, state->header.stamp , transform);
            }
            catch (tf::TransformException ex) {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }
            for (unsigned int j=0; j<3; j++) {
//.........这里部分代码省略.........