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

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

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


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

示例1: Test

void RobotAlgos::Test(void)
{
	    //Definition of a kinematic chain & add segments to the chain
    KDL::Chain chain;
    chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
    chain.addSegment(Segment(Joint(Joint::RotZ)));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
    chain.addSegment(Segment(Joint(Joint::RotZ)));
 
    // Create solver based on kinematic chain
    ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
 
    // Create joint array
    unsigned int nj = chain.getNrOfJoints();
    KDL::JntArray jointpositions = JntArray(nj);
 
    // Assign some values to the joint positions
    for(unsigned int i=0;i<nj;i++){
        float myinput;
        printf ("Enter the position of joint %i: ",i);
        scanf ("%e",&myinput);
        jointpositions(i)=(double)myinput;
    }
 
    // Create the frame that will contain the results
    KDL::Frame cartpos;    
 
    // Calculate forward position kinematics
    int kinematics_status;
    kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
    if(kinematics_status>=0){
        std::cout << cartpos <<std::endl;
        printf("%s \n","Succes, thanks KDL!");
    }else{
        printf("%s \n","Error: could not calculate forward kinematics :(");
    }


	//Creation of the solvers:
	ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
	ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
	ChainIkSolverPos_NR iksolver1(chain,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
	 
	//Creation of jntarrays:
	JntArray result(chain.getNrOfJoints());
	JntArray q_init(chain.getNrOfJoints());
	 
	//Set destination frame
	Frame F_dest=cartpos;
	 
	iksolver1.CartToJnt(q_init,F_dest,result);

	for(unsigned int i=0;i<nj;i++){
        printf ("Axle %i: %f \n",i,result(i));
    }


}
开发者ID:PrLayton，项目名称:SeriousFractal，代码行数:60，代码来源:RobotAlgos.cpp

示例2: LWR_knife

KDL::Chain kukaControl::LWR_knife() {
	KDL::Chain chain;

	// base
	chain.addSegment(
	// KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame::DH_Craig1989(0, 0, 0.33989, 0)));
			KDL::Segment(KDL::Joint(KDL::Joint::None),
					KDL::Frame::DH_Craig1989(0, 0, 0.34, 0)));
	// joint 1
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
	// joint 2
	chain.addSegment(
	// KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40011, 0)));
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40, 0)));
	// joint 3
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));
	// joint 4
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0.4000, 0)));
	// joint 5
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
	// joint 6
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));

	// joint 7 (with flange adapter)
	/*  chain.addSegment(
	 KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
	 KDL::Frame::DH_Craig1989(0, 0, 0.12597, 0)));*/
	// for HSC
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, 0, 0.12597 + 0.41275, 0)));

	KDL::Frame ee = KDL::Frame(KDL::Vector(0.0625, -0.0925, -0.07))
			* KDL::Frame(
					KDL::Rotation::EulerZYX(32.0 * M_PI / 180, 0.0,
							54.18 * M_PI / 180));
	chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), ee));

	return chain;

}
开发者ID:handsomeboy，项目名称:ENPM661-Planning-for-Autonomous-Robots，代码行数:52，代码来源:kukaControl.cpp

示例3: LWR_HSC_nozzle_RCM_used

KDL::Chain kukaControl::LWR_HSC_nozzle_RCM_used() {
	KDL::Chain chain;
	// base
	chain.addSegment(
	// KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame::DH_Craig1989(0, 0, 0.33989, 0)));
			KDL::Segment(KDL::Joint(KDL::Joint::None),
					KDL::Frame::DH_Craig1989(0, 0, 0.34, 0)));
          //KDL::Frame::DH_Craig1989(0, M_PI_2, 0.36, 0)));
	// joint 1
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
	// joint 2
	chain.addSegment(
	// KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40011, 0)));
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40, 0)));
	          //KDL::Frame::DH_Craig1989(0, M_PI_2, 0.42, 0)));
	// joint 3
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));
	// joint 4
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0.4000, 0)));
	// joint 5
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
	// joint 6
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));

	// joint 7 (with flange adapter)
	/*  chain.addSegment(
	 KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
	 KDL::Frame::DH_Craig1989(0, 0, 0.12597, 0)));*/
	// for HSC
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, 0,
							0.12597 + Dist_from_EndeffectorToRcmPoint, 0)));

	return chain;

}
开发者ID:handsomeboy，项目名称:ENPM661-Planning-for-Autonomous-Robots，代码行数:48，代码来源:kukaControl.cpp

示例4: cartPosInputHandle

    bool kuka_IK::cartPosInputHandle(RTT::base::PortInterface* portInterface){
    	input_cartPosPort.read(commandedPose);
#if DEBUG
    	cout << endl << "Pose.position.x  = " << commandedPose.position.x << endl;
    	cout << "Pose.position.y  = " << commandedPose.position.y << endl;
    	cout << "Pose.position.z  = " << commandedPose.position.z << endl;
#endif
    	// Read out the robot joint position
    	msr_jntPosPort.read(jntPos);


#if DEBUG
    	std::cout << " kuka-IK-component.cpp: jntPos" << std::endl;
   		for(int i = 0; i < 7; i++)  std::cout << jntPos[i] << " " ;
#endif
    	//Do IK and reset velocity profiles
    	commndedPoseJntPos = std::vector<double>(7,0.0);

#if DEBUG
    	cout <<  endl << endl;
    	cout << "Beginning of Super Debugging" << endl;
    	geometry_msgs::Pose tmpPose;
    	cartPosPort.read(tmpPose);
    	Frame tmpFrame;
    	tf::PoseMsgToKDL(tmpPose, tmpFrame);

    	cout << "Frame from Robot" << endl << tmpFrame << endl;

    	//Debugging Iterative IK
		KDL::Chain chain = Chain();
		chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Rotation::RotZ(M_PI), Vector(0.0, 0.0, 0.31))));
		chain.addSegment(Segment(Joint(Joint::RotY), Frame(Rotation::RotZ(M_PI), Vector(0.0, 0.0, 0.2))));
		chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.2))));
		chain.addSegment(Segment(Joint(Joint::RotY), Frame(Rotation::RotZ(M_PI), Vector(0.0, 0.0, 0.2))));
		chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.19))));
		chain.addSegment(Segment(Joint(Joint::RotY), Frame(Rotation::RotZ(M_PI))));
		chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.078))));

		ChainFkSolverPos_recursive fksolver(chain);

		// Create joint array
		unsigned int nj = chain.getNrOfJoints();
		KDL::JntArray jointpositions = JntArray(nj);

		// Assign some values to the joint positions
		std::cout << "jntPos: ";
		for(unsigned int i=0;i<nj;i++){
			jointpositions(i)=jntPos[i];
			std::cout << jntPos[i] << " ";
		}
		std::cout << std::endl;

		// Create the frame that will contain the results
		KDL::Frame cartpos;

		// Calculate forward position kinematics
		bool kinematics_status;
		kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
		if(kinematics_status>=0){
			std::cout << cartpos <<std::endl;
			printf("%s \n","Success, thanks KDL!");
		}else{
			printf("%s \n","Error: could not calculate forward kinematics");
			std::cout << cartpos <<std::endl;
		}

		cout << "End of Super Debugging" << endl << endl;

		//End of Super Debugging
#endif

		//if (! KukaLWR_Kinematics::ikSolverAnalytical7DOF( commandedPose, commndedPoseJntPos) ){
    	//if (!(KukaLWR_Kinematics::ikSolver(jntPos, commandedPose, commndedPoseJntPos))){
    	if (!(KukaLWR_Kinematics::ikSolverIterative7DOF(jntPos, commandedPose, commndedPoseJntPos))){
    		cout << "lastCommandedPose cannot be achieved" << endl;
    		for(int i = 0; i < 7; i++)  std::cout << commndedPoseJntPos[i] << " " ;
    		std::cout << std::endl;
    	}
        
        sensor_msgs::JointState tmpJntState;
        tmpJntState.position.clear();
        for(int i=0; i < 7; i++){
    		tmpJntState.position.push_back(commndedPoseJntPos[i]);
    		tmpJntState.velocity.push_back(0.0);
    	}

    	output_jntPosPort.write(tmpJntState);
    	return true;

    }
开发者ID:geoz，项目名称:trajectory-generator，代码行数:90，代码来源:kuka_IK-component.cpp

示例5: outdate

KukaKDL::KukaKDL(){
    segment_map.push_back("base");
    segment_map.push_back("00");
    segment_map.push_back("01");
    segment_map.push_back("02");
    segment_map.push_back("03");
    segment_map.push_back("04");
    segment_map.push_back("05");
    segment_map.push_back("06");
    segment_map.push_back("07");

    KDL::Chain chain;

    //joint 0
    chain.addSegment(KDL::Segment("base", KDL::Joint(KDL::Joint::None),
                KDL::Frame::DH_Craig1989(0.0, ALPHA0, R0, 0.0)
                ));

    //joint 1
	chain.addSegment(KDL::Segment("00", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0, ALPHA1, R1, 0.0),
                KDL::Frame::DH_Craig1989(0.0, ALPHA1, R1, 0.0).Inverse()*KDL::RigidBodyInertia(M1_KDL,
                    KDL::Vector(COGX1_KDL, COGY1_KDL, COGZ1_KDL),
                    KDL::RotationalInertia(XX1_KDL,YY1_KDL,ZZ1_KDL,XY1_KDL,XZ1_KDL,YZ1_KDL))));

    //joint 2 
    chain.addSegment(KDL::Segment("01", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA2, R2, 0.0),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA2, R2, 0.0).Inverse()*KDL::RigidBodyInertia(M2_KDL,
                    KDL::Vector(COGX2_KDL, COGY2_KDL, COGZ2_KDL),
                    KDL::RotationalInertia(XX2_KDL,YY2_KDL,ZZ2_KDL,XY2_KDL,XZ2_KDL,YZ2_KDL))));

    //joint 3
    chain.addSegment(KDL::Segment("02", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA3, R3, 0.0),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA3, R3, 0.0).Inverse()*KDL::RigidBodyInertia(M3_KDL,
                    KDL::Vector(COGX1_KDL, COGY1_KDL, COGZ1_KDL),
                    KDL::RotationalInertia(XX1_KDL,YY1_KDL,ZZ1_KDL,XY1_KDL,XZ1_KDL,YZ1_KDL))));

    //joint 4
    chain.addSegment(KDL::Segment("03", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0, ALPHA4, R4, 0.0),
                KDL::Frame::DH_Craig1989(0.0, ALPHA4, R4, 0.0).Inverse()*KDL::RigidBodyInertia(M4_KDL,
                    KDL::Vector(COGX4_KDL, COGY4_KDL, COGZ4_KDL),
                    KDL::RotationalInertia(XX4_KDL,YY4_KDL,ZZ4_KDL,XY4_KDL,XZ4_KDL,YZ4_KDL))));

    //joint 5
    chain.addSegment(KDL::Segment("04", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA5, R5, 0.0),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA5, R5, 0.0).Inverse()*KDL::RigidBodyInertia(M5_KDL,
                    KDL::Vector(COGX5_KDL, COGY5_KDL, COGZ5_KDL),
                    KDL::RotationalInertia(XX5_KDL,YY5_KDL,ZZ5_KDL,XY5_KDL,XZ5_KDL,YZ5_KDL))));

    //joint 6
    chain.addSegment(KDL::Segment("05", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0, ALPHA6, R6, 0.0),
                KDL::Frame::DH_Craig1989(0.0, ALPHA6, R6, 0.0).Inverse()*KDL::RigidBodyInertia(M6_KDL,
                    KDL::Vector(COGX6_KDL, COGY6_KDL, COGZ6_KDL),
                    KDL::RotationalInertia(XX6_KDL,YY6_KDL,ZZ6_KDL,XY6_KDL,XZ6_KDL,YZ6_KDL))));
    //joint 7
    chain.addSegment(KDL::Segment("06", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0, ALPHA7, R7, 0.0),
                KDL::Frame::DH_Craig1989(0.0, ALPHA7, R7, 0.0).Inverse()*KDL::RigidBodyInertia(M7_KDL,
                    KDL::Vector(COGX7_KDL, COGY7_KDL, COGZ7_KDL),
                    KDL::RotationalInertia(XX7_KDL,YY7_KDL,ZZ7_KDL,XY7_KDL,XZ7_KDL,YZ7_KDL))));

    //tool
    chain.addSegment(KDL::Segment("07", KDL::Joint(KDL::Joint::None), KDL::Frame::Identity()));
    

    tree.addChain(chain, "root");

    treejacsolver = new KDL::TreeJntToJacSolver(tree);
    fksolver = new KDL::TreeFkSolverPos_recursive(tree);
    fksolvervel = new KDL::ChainFkSolverVel_recursive(chain);

    q.resize(chain.getNrOfJoints());


	dynModelSolver = new KDL::ChainDynParam(chain,KDL::Vector(0.,0.,-9.81));
    qd.resize(chain.getNrOfJoints());
    externalWrenchTorque.resize(chain.getNrOfJoints());
    massMatrix.resize(chain.getNrOfJoints());
    corioCentriTorque.resize(chain.getNrOfJoints());
    gravityTorque.resize(chain.getNrOfJoints());
    frictionTorque.resize(chain.getNrOfJoints());

    actuatedDofs = Eigen::VectorXd::Constant(tree.getNrOfJoints(), 1);
    lowerLimits.resize(tree.getNrOfJoints());
    lowerLimits << -2.97, -2.10, -2.97, -2.10, -2.97, -2.10, -2.97;
    upperLimits.resize(tree.getNrOfJoints());
    upperLimits << 2.97, 2.10, 2.97, 2.10, 2.97, 2.10, 2.97;


    outdate();
}
开发者ID:ISIR-SYROCO，项目名称:kukaKDL，代码行数:96，代码来源:kukakdl.cpp


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