bool checkURDF2DHForAGivenChain(const KDL::CoDyCo::UndirectedTree& undirectedTree, const std::string& base_frame, const std::string& end_effector_frame) { KDL::Chain kdl_chain; iCub::iKin::iKinLimb ikin_limb; //cerr << "urdf2dh test testing chain extraction between " << base_frame // << " and " << end_effector_frame << endl; KDL::Tree kdl_rotated_tree = undirectedTree.getTree(base_frame); bool result = kdl_rotated_tree.getChain(base_frame,end_effector_frame,kdl_chain); if( !result ) { cerr << "urdf2dh: Impossible to find " << base_frame << " or " << end_effector_frame << " in the URDF." << endl; return false; } // // Convert the chain and the limits to an iKin chain (i.e. DH parameters) // KDL::JntArray qmin(kdl_chain.getNrOfJoints()),qmax(kdl_chain.getNrOfJoints()); for(size_t i=0; i < qmin.rows(); i++ ) { qmin(i) = -10.0; qmax(i) = 10.0; } result = iKinLimbFromKDLChain(kdl_chain,ikin_limb,qmin,qmax,1000); if( !result ) { cerr << "urdf2dh: Could not export KDL::Tree to iKinChain" << endl; return false; } return checkChainsAreEqual(kdl_chain,ikin_limb); }
int main(int argc, char** argv) { if ( argc == 2 ) { if( (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h" ) ) { printHelp(); return 0; } } if (argc != 5) { printHelp(); return -1; } std::string urdf_file_name = argv[1]; std::string base_link_name = argv[2]; std::string end_effector_link_name = argv[3]; std::string ikin_ini_file_name = argv[4]; KDL::Tree kdl_tree; KDL::Chain kdl_chain; iCub::iKin::iKinLimb ikin_limb; std::vector<std::string> joint_names; KDL::JntArray min,max; // // URDF --> KDL::Tree // bool root_inertia_workaround = true; if( !treeFromUrdfFile(urdf_file_name,kdl_tree,root_inertia_workaround) ) { cerr << "urdf2dh: Could not parse urdf robot model" << endl; std::cerr << "urdf2dh: Please open an issue at https://github.com/robotology-playground/idyntree/issues " << std::endl; return EXIT_FAILURE; } // // URDF --> position ranges // if( !jointPosLimitsFromUrdfFile(urdf_file_name,joint_names,min,max) ) { cerr << "Could not parse urdf robot model limits" << endl; return EXIT_FAILURE; } if( joint_names.size() != min.rows() || joint_names.size() != max.rows() || joint_names.size() == 0) { cerr << "Inconsistent joint limits got from urdf (nr of joints extracted: " << joint_names.size() << " ) " << endl; return EXIT_FAILURE; } // // KDL::Tree --> KDL::CoDyCo::UndirectedTree // (for extracting arbitrary chains, // using KDL::Tree you can just get chains where the base of the chain // is proximal to the tree base with respect to the end effector. // KDL::CoDyCo::UndirectedTree undirected_tree(kdl_tree); KDL::Tree kdl_rotated_tree = undirected_tree.getTree(base_link_name); bool result = kdl_rotated_tree.getChain(base_link_name,end_effector_link_name,kdl_chain); if( !result ) { cerr << "urdf2dh: Impossible to find " << base_link_name << " or " << end_effector_link_name << " in the URDF." << endl; return EXIT_FAILURE; } // // Copy the limits extracted from the URDF to the chain // int nj = kdl_chain.getNrOfJoints(); KDL::JntArray chain_min(nj), chain_max(nj); size_t seg_i, jnt_i; for(seg_i=0,jnt_i=0; seg_i < kdl_chain.getNrOfSegments(); seg_i++) { const Segment & seg = kdl_chain.getSegment(seg_i); if( seg.getJoint().getType() != KDL::Joint::None ) { std::string jnt_name = seg.getJoint().getName(); // std::cerr << "searching for joint " << jnt_name << std::endl; int tree_jnt = 0; for(tree_jnt = 0; tree_jnt < joint_names.size(); tree_jnt++ ) { //std::cerr << "joint_names[ " << tree_jnt << "] is " << joint_names[tree_jnt] << std::endl; if( joint_names[tree_jnt] == jnt_name ) { chain_min(jnt_i) = min(tree_jnt); chain_max(jnt_i) = max(tree_jnt); jnt_i++; break; } } if( tree_jnt == joint_names.size() ) { std::cerr << "urdf2dh failure in converting limits from tree to chain, unable to find joint " << jnt_name << std::endl; return EXIT_FAILURE; } } } if( jnt_i != nj ) { std::cerr << "urdf2dh failure in converting limits from tree to chain" << std::endl; return EXIT_FAILURE; } // // Convert the chain and the limits to an iKin chain (i.e. DH parameters) // result = iKinLimbFromKDLChain(kdl_chain,ikin_limb,chain_min,chain_max); if( !result ) { cerr << "urdf2dh: Could not export KDL::Tree to iKinChain" << endl; return EXIT_FAILURE; } bool result_corrupted = false; if( !checkChainsAreEqual(kdl_chain,ikin_limb) ) { cerr << "urdf2dh error: KDL::Chain and iKinChain results does not match" << endl; std::cerr << "urdf2dh: Please open an issue at https://github.com/robotology-playground/idyntree/issues " << std::endl; return false; } yarp::os::Property prop; result = ikin_limb.toLinksProperties(prop); if( !result ) { cerr << "urdf2dh: Could not export Link Properties from ikin_limb" << endl; return EXIT_FAILURE; } else { std::cout << "urdf2dh: Conversion to iKin DH chain completed correctly" << std::endl; } std::string ikin_prop = prop.toString(); yarp::os::Bottle prop_bot; prop_bot.fromString(prop.toString()); //Write the properties to file std::ofstream ofs (ikin_ini_file_name.c_str(), std::ofstream::out); ofs << prop_bot.findGroup("type").toString() << std::endl; ofs << prop_bot.findGroup("numLinks").toString() << std::endl; ofs << prop_bot.findGroup("H0").toString() << std::endl; for( int link = 0; link < ikin_limb.getN(); link++ ) { std::string link_name = "link_" + int2string(link); ofs << prop_bot.findGroup(link_name).toString() << std::endl; } ofs << prop_bot.findGroup("HN").toString() << std::endl; ofs.close(); if( result_corrupted ) { return EXIT_FAILURE; } else { return EXIT_SUCCESS; } }