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