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


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

}
Exemple #3
0
bool addBaseTransformation(const KDL::Chain & old_chain, KDL::Chain & new_chain, KDL::Frame H_new_old)
{
    new_chain = KDL::Chain();
    for(unsigned int i=0; i<old_chain.getNrOfSegments(); i++) {
        KDL::Segment segm;
        segm = old_chain.getSegment(i);
        //if is not the first segment add normally the segment
        if( i != 0 ) {
            new_chain.addSegment(segm);
        } else {
            //otherwise modify the segment before adding it
            KDL::Segment new_segm;
            KDL::Joint new_joint, old_joint;
            old_joint = segm.getJoint();
            KDL::Joint::JointType new_type;
            switch(old_joint.getType()) {
            case KDL::Joint::RotAxis:
            case KDL::Joint::RotX:
            case KDL::Joint::RotY:
            case KDL::Joint::RotZ:
                new_type = KDL::Joint::RotAxis;
                break;
            case KDL::Joint::TransAxis:
            case KDL::Joint::TransX:
            case KDL::Joint::TransY:
            case KDL::Joint::TransZ:
                new_type = KDL::Joint::TransAxis;
                break;
            case KDL::Joint::None:
            default:
                new_type = KDL::Joint::None;
            }

            //check !

            new_joint = KDL::Joint(old_joint.getName(),H_new_old*old_joint.JointOrigin(),H_new_old.M*old_joint.JointAxis(),new_type);
            new_segm = KDL::Segment(segm.getName(),new_joint,H_new_old*segm.getFrameToTip(),segm.getInertia());
            new_chain.addSegment(new_segm);
        }
    }
    return true;
}
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;

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

    }
Exemple #6
0
bool idynChain2kdlChain(iCub::iDyn::iDynChain & idynChain,KDL::Chain & kdlChain,std::vector<std::string> link_names,std::vector<std::string> joint_names, std::string final_frame_name, std::string initial_frame_name, int max_links)
{
    int n_links, i;
    bool use_names;
    n_links = idynChain.getN();
    //In iDyn, the joints are only 1 DOF (not 0, not more)
    int n_joints = idynChain.getN();

    if(n_links <= 0 ) return false;

    if( (int)link_names.size() < n_links || (int)joint_names.size() < n_joints ) {
        use_names = false;
    } else {
        use_names = true;
    }


    KDL::Frame kdlFrame = KDL::Frame();
    KDL::Frame kdl_H = KDL::Frame();
    KDL::Joint kdlJoint = KDL::Joint();
    kdlChain = KDL::Chain();
    KDL::Segment kdlSegment = KDL::Segment();
    KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia();


    if( initial_frame_name.length() != 0 ) {
        idynMatrix2kdlFrame(idynChain.getH0(),kdlFrame);
        kdlSegment = KDL::Segment(initial_frame_name,KDL::Joint(initial_frame_name+"_joint",KDL::Joint::None),kdlFrame);
        kdlChain.addSegment(kdlSegment);
    }


    for(i=0; i<n_links && i < max_links; i++)
    {
        //forgive him, as he does not know what is doing
        iCub::iKin::iKinLink & link_current = idynChain[i];
        //For the first link and the last link, take in account also H0 and HN
        if ( i == n_links - 1) {
            if( final_frame_name.length() == 0 ) {
                idynMatrix2kdlFrame(idynChain.getHN(),kdl_H);
                kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset())*kdl_H;
            } else {
                kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
            }
        } else {
            kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
        }


        bool ret = idynDynamicalParameters2kdlRigidBodyInertia(idynChain.getMass(i),idynChain.getCOM(i).subcol(0,3,3),idynChain.getInertia(i),kdlRigidBodyInertia);
        if( !ret ) return false;

        KDL::Joint jnt_idyn;

        //\todo: joint can also be blocked at values different from 0.0
        if( idynChain.isLinkBlocked(i) ) {
            if( use_names ) {
                kdlSegment = KDL::Segment(link_names[i],KDL::Joint(joint_names[i],KDL::Joint::None),kdlFrame,kdlRigidBodyInertia);
            } else {
                kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),kdlFrame,kdlRigidBodyInertia);
            }
        } else {
            if( use_names ) {
                kdlSegment = KDL::Segment(link_names[i],KDL::Joint(joint_names[i],KDL::Joint::RotZ),kdlFrame,kdlRigidBodyInertia);
            } else {
                kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),kdlFrame,kdlRigidBodyInertia);
            }
        }
        kdlChain.addSegment(kdlSegment);


    }

    //if specified, add a final fake link
    if( final_frame_name.length() != 0 ) {
        idynMatrix2kdlFrame(idynChain.getHN(),kdlFrame);
        kdlSegment = KDL::Segment(final_frame_name,KDL::Joint(final_frame_name+"_joint",KDL::Joint::None),kdlFrame);
        kdlChain.addSegment(kdlSegment);
    }

    //Considering the H0 transformation
    if( initial_frame_name.length() == 0 ) {
        KDL::Chain new_chain;
        KDL::Frame kdl_H0;
        idynMatrix2kdlFrame(idynChain.getH0(),kdl_H0);
        //std::cout << "KDL_h0 " <<  kdl_H0 << std::endl;

        addBaseTransformation(kdlChain,new_chain,kdl_H0);
        kdlChain = new_chain;
    }

    return true;
}
Exemple #7
0
bool idynSensorChain2kdlChain(iCub::iDyn::iDynChain & idynChain,
                              iCub::iDyn::iDynInvSensor & idynSensor,
                              KDL::Chain & kdlChain,
                              KDL::Frame & H_sensor_dh_child,
                              std::vector<std::string> link_names,std::vector<std::string> joint_names, std::string final_frame_name, std::string initial_frame_name, int max_links)
{
    bool use_names;
    int n_links, i, sensor_link;
    n_links = idynChain.getN();
    sensor_link = idynSensor.getSensorLink();

    int kdl_links = n_links + 1; //The sensor links transform a link in two different links
    int kdl_joints = kdl_links;

    if(n_links <= 0 ) return false;

    if( (int)link_names.size() < kdl_links || (int)joint_names.size() < kdl_joints ) {
        use_names = false;
    } else {
        use_names = true;
    }


    KDL::Frame kdlFrame = KDL::Frame();
    KDL::Frame kdl_H;
    kdlChain = KDL::Chain();
    KDL::Segment kdlSegment = KDL::Segment();
    KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia();

    int kdl_i = 0;

    if( initial_frame_name.length() != 0 ) {
        idynMatrix2kdlFrame(idynChain.getH0(),kdlFrame);
        kdlSegment = KDL::Segment(initial_frame_name,KDL::Joint(initial_frame_name+"_joint",KDL::Joint::None),kdlFrame);
        kdlChain.addSegment(kdlSegment);
    }


    KDL::Frame remainder_frame = KDL::Frame::Identity();
    for(i=0; i<n_links; i++)
    {
        if( i != sensor_link ) {
            //forgive him, as he does not know what is doing
            iCub::iKin::iKinLink & link_current = idynChain[i];
            //For the last link, take in account also HN
            if ( i == n_links - 1) {
                idynMatrix2kdlFrame(idynChain.getHN(),kdl_H);
                kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset())*kdl_H;
            } else {
                kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
            }

            bool ret = idynDynamicalParameters2kdlRigidBodyInertia(idynChain.getMass(i),idynChain.getCOM(i).subcol(0,3,3),idynChain.getInertia(i),kdlRigidBodyInertia);
            assert(ret);
            if(!ret) return false;

            //For the joint after the ft sensor, consider the offset between the ft sensor
            // and the joint

            if( idynChain.isLinkBlocked(i) ) {
                // if it is blocked, it is easy to add the remainder frame
                if( use_names ) {
                    kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
                    kdl_i++;
                } else {
                    kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
                }
            } else {
                KDL::Vector new_joint_axis, new_joint_origin;
                new_joint_axis = remainder_frame.M*KDL::Vector(0.0,0.0,0.1);
                new_joint_origin = remainder_frame.p;
                if( use_names ) {
                    kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
                    kdl_i++;
                } else {
                    kdlSegment = KDL::Segment(KDL::Joint(new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
                }
            }
            kdlChain.addSegment(kdlSegment);
            remainder_frame = KDL::Frame::Identity();
        } else {
            //( i == segment_link )
            double m,m0,m1;
            yarp::sig::Vector r0(3),r1(3),r(3),rgg0(3),rgg1(3),r_i_s_wrt_i(3),r_i_C0_wrt_i;
            yarp::sig::Matrix I,I0,I1;
            yarp::sig::Matrix R_s_wrt_i;

            iCub::iKin::iKinLink & link_current = idynChain[sensor_link];
            KDL::Frame kdlFrame_0 = KDL::Frame();
            KDL::Frame kdlFrame_1 = KDL::Frame();

            //Imagine that we have i, s , i+1

            //yarp::sig::Matrix H_0;
            //The angle of the sensor link joint is put to 0 and then restored
            double angSensorLink = link_current.getAng();
            yarp::sig::Matrix  H_sensor_link = (link_current.getH(0.0)); //H_{i-1}_i
            link_current.setAng(angSensorLink);
            //idynSensor.getH() <--> H_i_s
            yarp::sig::Matrix H_0 = H_sensor_link  * (idynSensor.getH()); // H_{i-1}_s = H_{i-1}_i*H_i_s ?
            yarp::sig::Matrix H_1 = localSE3inv(idynSensor.getH()); //H_s_{i}
            //std::cout << "H_0" << std::endl << H_0.toString() << std::endl;
            //std::cout << "H_1" << std::endl << H_1.toString() << std::endl;
            idynMatrix2kdlFrame(H_0,kdlFrame_0);
            idynMatrix2kdlFrame(H_1,kdlFrame_1);

            remainder_frame = kdlFrame_1;

            H_sensor_dh_child = remainder_frame;

            kdlFrame_1 = KDL::Frame::Identity();

            //cout << "kdlFrame_0: " << endl;
            //cout << kdlFrame_0;
            //cout << "kdlFrame_1: " << endl;
            //cout << kdlFrame_1;


            KDL::RigidBodyInertia kdlRigidBodyInertia_0 = KDL::RigidBodyInertia();
            KDL::RigidBodyInertia kdlRigidBodyInertia_1 = KDL::RigidBodyInertia();

            m = idynChain.getMass(sensor_link);
            m1 = idynSensor.getMass();
            m0 = m-m1;
            //It is not possible that the semilink is more heavy than the link!!!
            assert(m0 > 0);

            //r_{i,C_i}^i
            r = idynChain.getCOM(i).subcol(0,3,3);

            //R_s^i
            R_s_wrt_i = idynSensor.getH().submatrix(0,2,0,2);

            r_i_s_wrt_i = idynSensor.getH().subcol(0,3,3);

            //r0 := r_{s,C_{{vl}_0}}^s
            //r1 := r_{i,C_{{vl}_1}}^i



            //
            r1 = r_i_s_wrt_i +  R_s_wrt_i*(idynSensor.getCOM().subcol(0,3,3));

            //cout << "m0: " << m0 << endl;
            r_i_C0_wrt_i = (1/m0)*(m*r-m1*r1);

            r0 = R_s_wrt_i.transposed()*(r_i_C0_wrt_i-r_i_s_wrt_i);

            I = idynChain.getInertia(i);

            I1 = R_s_wrt_i*idynSensor.getInertia()*R_s_wrt_i.transposed();
            rgg0 = -1*r+r_i_C0_wrt_i;
            rgg1 = -1*r+r1;

            I0 = R_s_wrt_i.transposed()*(I - I1 + m1*crossProductMatrix(rgg1)*crossProductMatrix(rgg1) +  m0*crossProductMatrix(rgg0)*crossProductMatrix(rgg0))*R_s_wrt_i;

            //DEBUG
            //printMatrix("I",I);
            //printMatrix("I1",I1);
            //printMatrix("I0",I0);
            //printMatrix("cross",crossProductMatrix(rgg1));

            //cout << "m0: " << m0 << endl;
            //printVector("r0",r0);
            //printMatrix("I0",I0);
            bool ret;
            ret = idynDynamicalParameters2kdlRigidBodyInertia(m0,r0,I0,kdlRigidBodyInertia_0);
            assert(ret);
            if(!ret) return false;
            ret = idynDynamicalParameters2kdlRigidBodyInertia(m1,r1,I1,kdlRigidBodyInertia_1);
            assert(ret);
            if(!ret) return false;

            KDL::RigidBodyInertia kdlRigidBodyInertia_1_buf;
            kdlRigidBodyInertia_1_buf = remainder_frame*kdlRigidBodyInertia_1;
            kdlRigidBodyInertia_1 = kdlRigidBodyInertia_1_buf;

            if( use_names ) {
                kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::RotZ),kdlFrame_0,kdlRigidBodyInertia_0);
                kdl_i++;
            } else {
                kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),kdlFrame_0,kdlRigidBodyInertia_0);
            }
            kdlChain.addSegment(kdlSegment);

            if( use_names ) {
                kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),kdlFrame_1,kdlRigidBodyInertia_1);
                kdl_i++;
            } else {
                kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),kdlFrame_1,kdlRigidBodyInertia_1);
            }
            kdlChain.addSegment(kdlSegment);
        }

    }
    //Final link can be segment
    if( final_frame_name.length() != 0 ) {
        idynMatrix2kdlFrame(idynChain.getHN(),kdlFrame);
        kdlSegment = KDL::Segment(final_frame_name,KDL::Joint(final_frame_name+"_joint",KDL::Joint::None),kdlFrame);
        kdlChain.addSegment(kdlSegment);
    }

    if( max_links < (int)kdlChain.getNrOfSegments() )
    {
        KDL::Chain new_kdlChain;
        for(int p=0; p < max_links; p++)
        {
            new_kdlChain.addSegment(kdlChain.getSegment(p));
        }
        kdlChain = new_kdlChain;
    }

    //Considering the H0 transformation
    if( initial_frame_name.length() == 0 ) {
        KDL::Chain new_chain;
        KDL::Frame kdl_H0;
        idynMatrix2kdlFrame(idynChain.getH0(),kdl_H0);
        addBaseTransformation(kdlChain,new_chain,kdl_H0);
        kdlChain = new_chain;
    }

    return true;
}
Exemple #8
0
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();
}