当前位置: 首页>>代码示例>>C++>>正文

C++ Frame::p方法代码示例

本文整理汇总了C++中kdl::Frame::p方法的典型用法代码示例。如果您正苦于以下问题：C++ Frame::p方法的具体用法？C++ Frame::p怎么用？C++ Frame::p使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类kdl::Frame的用法示例。


在下文中一共展示了Frame::p方法的7个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: set_marker

	void MultiTaskPriorityInverseKinematics::set_marker(KDL::Frame x, int index, int id)
	{			
				sstr_.str("");
				sstr_.clear();

				if (links_index_[index] == -1)
					sstr_<<"end_effector";		
				else
					sstr_<<"link_"<<(links_index_[index]-1);


				msg_marker_.markers[index].header.frame_id = "world";
				msg_marker_.markers[index].header.stamp = ros::Time();
				msg_marker_.markers[index].ns = sstr_.str();
				msg_marker_.markers[index].id = id;
				msg_marker_.markers[index].type = visualization_msgs::Marker::SPHERE;
				msg_marker_.markers[index].action = visualization_msgs::Marker::ADD;
				msg_marker_.markers[index].pose.position.x = x.p(0);
				msg_marker_.markers[index].pose.position.y = x.p(1);
				msg_marker_.markers[index].pose.position.z = x.p(2);
				msg_marker_.markers[index].pose.orientation.x = 0.0;
				msg_marker_.markers[index].pose.orientation.y = 0.0;
				msg_marker_.markers[index].pose.orientation.z = 0.0;
				msg_marker_.markers[index].pose.orientation.w = 1.0;
				msg_marker_.markers[index].scale.x = 0.01;
				msg_marker_.markers[index].scale.y = 0.01;
				msg_marker_.markers[index].scale.z = 0.01;
				msg_marker_.markers[index].color.a = 1.0;
				msg_marker_.markers[index].color.r = 0.0;
				msg_marker_.markers[index].color.g = 1.0;
				msg_marker_.markers[index].color.b = 0.0;	
	}
开发者ID:CentroEPiaggio，项目名称:kuka-lwr，代码行数:32，代码来源:multi_task_priority_inverse_kinematics.cpp

示例2: getXInv

void ChainKinematics::getXInv(double (&t)[3], double (&rot)[9], const double* q, const int segmentNr) const {

	int i,j;

	KDL::JntArray qJA(Njnt_);
	for(i=0; i<Njnt_; i++)
		qJA(i) = q[i];

	KDL::Frame frame;

	//compute forward kinematics
	checkSegmentNr(segmentNr);
	if(fksolverPos_->JntToCart(qJA, frame, segmentNr) < 0) {
		std::cerr << "ERROR: [ChainKinematics][getXInv] something went wrong during JntToCart! Exiting!" << std::endl;
		exit(-1);
	}

    //get inverse transformation
    frame = frame.Inverse();

	for(i=0; i<3; i++) {
		t[i] = frame.p(i);
		for(j=0; j<3; j++)
			rot[i*3+j] = frame.M(i,j);
	}

}
开发者ID:fjp，项目名称:ba，代码行数:27，代码来源:ChainKinematics.cpp

示例3: getX

void ChainKinematics::getX(double (&t)[3], double (&rot)[9], const double* q, const int segmentNr) const {

	int i,j;

    ////DEBUG
    //std::cout << "++++++++++++++++++++ DEBUG 0" << std::endl;
    //std::cout << "q = ";
	//for(i=0; i<Njnt_; i++)
        //std::cout << q[i] << " " << std::endl;
    //std::cout << std::endl;


	KDL::JntArray qJA(Njnt_);
	for(i=0; i<Njnt_; i++)
		qJA(i) = q[i];

	KDL::Frame frame;

	//compute forward kinematics
	checkSegmentNr(segmentNr);
	if(fksolverPos_->JntToCart(qJA, frame, segmentNr) < 0) {
		std::cerr << "ERROR: [ChainKinematics][getX] something went wrong during JntToCart! Exiting!" << std::endl;
		exit(-1);
	}

	for(i=0; i<3; i++) {
		t[i] = frame.p(i);
		for(j=0; j<3; j++)
			rot[i*3+j] = frame.M(i,j);
	}

}
开发者ID:fjp，项目名称:ba，代码行数:32，代码来源:ChainKinematics.cpp

示例4: KDLToEigenMatrix

 Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
 {
   Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
   for(int i=0; i < 3; i++)
   {
     for(int j=0; j<3; j++)
     {
       b(i,j) = p.M(i,j);
     }
     b(i,3) = p.p(i);
   }
   return b;
 }
开发者ID:nttputus，项目名称:youbot-manipulation，代码行数:13，代码来源:arm_kinematics_constraint_aware_utils.cpp

示例5: set_marker

	void DynamicSlidingModeControllerTaskSpace::set_marker(KDL::Frame x, int id)
	{			
				msg_marker_.header.frame_id = "world";
				msg_marker_.header.stamp = ros::Time();
				msg_marker_.ns = "end_effector";
				msg_marker_.id = id;
				msg_marker_.type = visualization_msgs::Marker::SPHERE;
				msg_marker_.action = visualization_msgs::Marker::ADD;
				msg_marker_.pose.position.x = x.p(0);
				msg_marker_.pose.position.y = x.p(1);
				msg_marker_.pose.position.z = x.p(2);
				msg_marker_.pose.orientation.x = 0.0;
				msg_marker_.pose.orientation.y = 0.0;
				msg_marker_.pose.orientation.z = 0.0;
				msg_marker_.pose.orientation.w = 1.0;
				msg_marker_.scale.x = 0.005;
				msg_marker_.scale.y = 0.005;
				msg_marker_.scale.z = 0.005;
				msg_marker_.color.a = 1.0;
				msg_marker_.color.r = 0.0;
				msg_marker_.color.g = 1.0;
				msg_marker_.color.b = 0.0;	
	}
开发者ID:CentroEPiaggio，项目名称:kuka-lwr，代码行数:23，代码来源:dynamic_sliding_mode_controller_task_space.cpp

示例6: getQ

void ChainKinematics::getQ(double* q, const double* qInit, const double (&t)[3], const double (&rot)[9]) const {
	int i,j;

	KDL::JntArray qInitJA(Njnt_);
	for(i=0; i<Njnt_; i++)
		qInitJA(i) = qInit[i];

	KDL::Frame frame;
	for(i=0; i<3; i++) {
		frame.p(i) = t[i];
		for(j=0; j<3; j++)
			frame.M(i,j) = rot[i*3+j];
	}
	KDL::JntArray qJA;

	if(iksolverPos_->CartToJnt(qInitJA, frame, qJA) < 0) {
		std::cerr << "ERROR: [ChainKinematics][getQ] something went wrong during CartToJnt! Exiting!" << std::endl;
		exit(-1);
	}

	for(i=0; i<Njnt_; i++)
		q[i] = qJA(i);
}
开发者ID:fjp，项目名称:ba，代码行数:23，代码来源:ChainKinematics.cpp

示例7: ik

bool WamIkKdl::ik(vector<double> goal_in_cartesian, vector<double> currentjoints, vector<double>& goal_in_joints){

  //fk solver
  KDL::ChainFkSolverPos_recursive fksolver(KDL::ChainFkSolverPos_recursive(this->wam63_));

  // Create joint array
  KDL::JntArray setpointJP = KDL::JntArray(this->num_joints_);
  KDL::JntArray max = KDL::JntArray(this->num_joints_); //The maximum joint positions
  KDL::JntArray min = KDL::JntArray(this->num_joints_); //The minimium joint positions

  double minjp[7] = {-2.6,-2.0,-2.8,-0.9,-4.76,-1.6,-3.0};
  double maxjp[7] = { 2.6, 2.0, 2.8, 3.2, 1.24, 1.6, 3.0};

  for(unsigned int ii=0; ii < this->num_joints_; ii++){
    max(ii) = maxjp[ii];
    min(ii) = minjp[ii];
  }

  //Create inverse velocity solver
  KDL::ChainIkSolverVel_pinv_givens iksolverv = KDL::ChainIkSolverVel_pinv_givens(this->wam63_);
  
  //Iksolver Position: Maximum 100 iterations, stop at accuracy 1e-6
  //ChainIkSolverPos_NR iksolver = ChainIkSolverPos_NR(wam63,fksolver,iksolverv,100,1e-6);
  KDL::ChainIkSolverPos_NR_JL iksolver = KDL::ChainIkSolverPos_NR_JL(this->wam63_, min, max, fksolver, iksolverv, 2000, 1e-6); //With Joints Limits

  KDL::Frame cartpos;
  KDL::JntArray jointpos = KDL::JntArray(this->num_joints_);
  KDL::JntArray currentJP = KDL::JntArray(this->num_joints_);
  
  // Copying position to KDL frame
  cartpos.p(0) = goal_in_cartesian.at(3);
  cartpos.p(1) = goal_in_cartesian.at(7);
  cartpos.p(2) = goal_in_cartesian.at(11);

  // Copying Rotation to KDL frame
  cartpos.M(0,0) = goal_in_cartesian.at(0);
  cartpos.M(0,1) = goal_in_cartesian.at(1);
  cartpos.M(0,2) = goal_in_cartesian.at(2);
  cartpos.M(1,0) = goal_in_cartesian.at(4);
  cartpos.M(1,1) = goal_in_cartesian.at(5);
  cartpos.M(1,2) = goal_in_cartesian.at(6);
  cartpos.M(2,0) = goal_in_cartesian.at(8);
  cartpos.M(2,1) = goal_in_cartesian.at(9);
  cartpos.M(2,2) = goal_in_cartesian.at(10);

  for(unsigned int ii=0; ii < this->num_joints_; ii++)
    currentJP(ii) = currentjoints.at(ii);

  // Calculate inverse kinematics to go from currentJP to cartpos. The result in jointpos
  int ret = iksolver.CartToJnt(currentJP, cartpos, jointpos);

  if (ret >= 0) {

    std::cout << " Current Joint Position: [";
    for(unsigned int ii=0; ii < this->num_joints_; ii++)
      std::cout << currentJP(ii) << " ";
    std::cout << "]"<< std::endl;

    std::cout << "Cartesian Position " << cartpos << std::endl;

    std::cout << "IK result Joint Position: [";
    for(unsigned int ii=0; ii < this->num_joints_; ii++)
      std::cout << jointpos(ii) << " ";
    std::cout << "]"<< std::endl;

    goal_in_joints.clear();
    goal_in_joints.resize(this->num_joints_);
    for(unsigned int ii=0; ii < this->num_joints_; ii++)
      goal_in_joints[ii] = jointpos(ii);

  } else {

    std::cout << "Error: could not calculate inverse kinematics :(" << std::endl;
    return false;

  }

  return true;
}
开发者ID:dgerod，项目名称:WamKinematics，代码行数:79，代码来源:wam_ik_kdl.cpp


注：本文中的kdl::Frame::p方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。