bool framesFromKDLTree(const KDL::Tree& tree, std::vector<std::string>& framesNames, std::vector<std::string>& parentLinkNames) { framesNames.clear(); parentLinkNames.clear(); KDL::SegmentMap::iterator seg; KDL::SegmentMap segs; KDL::SegmentMap::const_iterator root_seg; root_seg = tree.getRootSegment(); segs = tree.getSegments(); for( seg = segs.begin(); seg != segs.end(); seg++ ) { if( GetTreeElementChildren(seg->second).size() == 0 && GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None && GetTreeElementSegment(seg->second).getInertia().getMass() == 0.0 ) { std::string frameName = GetTreeElementSegment(seg->second).getName(); std::string parentLinkName = GetTreeElementSegment(GetTreeElementParent(seg->second)->second).getName(); framesNames.push_back(frameName); parentLinkNames.push_back(parentLinkName); //also check parent KDL::Segment parent = GetTreeElementSegment(GetTreeElementParent(seg->second)->second); if (parent.getJoint().getType() == KDL::Joint::None && parent.getInertia().getMass() == 0.0) { framesNames.push_back(parentLinkName); } } } return true; }
int main() { KDLCollada kdlCollada; vector <KDL::Chain> kinematicsModels; const string filename = "puma.dae"; // loading collada kinematics model and converting it to kdl serial chain if (!kdlCollada.load(COLLADA_MODELS_PATH + filename, kinematicsModels)) { cout << "Failed to import " << filename; return 0; } cout << "Imported " << kinematicsModels.size() << " kinematics chains" << endl; for (unsigned int i = 0; i < kinematicsModels.size(); i++) // parsing output kdl serail chain { KDL::Chain chain = kinematicsModels[i]; cout << "Chain " << i << " has " << chain.getNrOfSegments() << " segments" << endl; for (unsigned int u = 0; u < chain.getNrOfSegments(); u++) { KDL::Segment segment = chain.segments[u]; string segmentName = segment.getName(); cout << "Segment " << segmentName << " :" <<endl; KDL::Frame f_tip = segment.getFrameToTip(); KDL::Vector rotAxis = f_tip.M.GetRot(); double rotAngle = f_tip.M.GetRotAngle(rotAxis); KDL::Vector trans = f_tip.p; cout << " frame: rotation " << rotAxis.x() << " " << rotAxis.y() << " " << rotAxis.z() << " " << rotAngle * 180 / M_PI << endl; cout << " frame: translation " << trans.x() << " " << trans.y() << " " << trans.z() << endl; KDL::RigidBodyInertia inertia = segment.getInertia(); KDL::Joint joint = segment.getJoint(); string jointName = joint.getName(); string jointType = joint.getTypeName(); KDL::Vector jointAxis = joint.JointAxis(); KDL::Vector jointOrigin = joint.JointOrigin(); cout << " joint name: " << jointName << endl; cout << " type: " << jointType << endl; cout << " axis: " << jointAxis.x() << " " <<jointAxis.y() << " " << jointAxis.z() << endl; cout << " origin: " << jointOrigin.x() << " " << jointOrigin.y() << " " << jointOrigin.z() << endl; } } return 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; }