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;
}
}
bool checkURDF2DH(std::string urdf_file_name)
{
std::cerr << "Running checkURDF2DH for model " << urdf_file_name << std::endl;
KDL::Tree kdl_tree;
//
// URDF --> KDL::Tree
//
bool root_inertia_workaround = true;
if( !treeFromUrdfFile(urdf_file_name,kdl_tree,root_inertia_workaround) )
{
cerr << "urdf2dh_tests: Could not parse urdf robot model" << endl;
std::cerr << "urdf2dh_tests: Please open an issue at https://github.com/robotology-playground/idyntree/issues " << std::endl;
return false;
}
//
// 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);
std::string base_frame;
std::string end_effector_frame;
bool test_success = true;
int success_count = 0;
int failure_count = 0;
std::vector<int> failure_as_base_frame(undirected_tree.getNrOfLinks(),0);
std::vector<int> failure_as_ee_frame(undirected_tree.getNrOfLinks(),0);
std::vector<int> success_as_base_frame(undirected_tree.getNrOfLinks(),0);
std::vector<int> success_as_ee_frame(undirected_tree.getNrOfLinks(),0);
for(size_t base_frame_index = 0;
base_frame_index < undirected_tree.getNrOfLinks();
base_frame_index++ )
{
for(size_t end_effector_frame_index = 0;
end_effector_frame_index < undirected_tree.getNrOfLinks();
end_effector_frame_index++ )
{
if( base_frame_index == end_effector_frame_index )
{
continue;
}
base_frame = undirected_tree.getLink(base_frame_index)->getName();
end_effector_frame = undirected_tree.getLink(end_effector_frame_index)->getName();
// There is a bug in undirected_tree.getChain("","base_link"), disregarding does result for now
if( base_frame == "base_link" ||
end_effector_frame == "base_link" )
{
continue;
}
cerr << "urdf2dh_test: Checking " << base_frame << " <--> " << end_effector_frame << " transform." << std::endl;
bool conversionOk = checkURDF2DHForAGivenChain(undirected_tree,base_frame,end_effector_frame);
if( !conversionOk )
{
cerr << "urdf2dh_test error: KDL::Chain and iKinChain between "
<< base_frame << " and " << end_effector_frame << " results does not match" << endl;
test_success = false;
failure_count++;
failure_as_base_frame[base_frame_index]++;
failure_as_ee_frame[end_effector_frame_index]++;
}
else
{
success_count++;
success_as_base_frame[base_frame_index]++;
success_as_ee_frame[end_effector_frame_index]++;
}
}
}
cerr << "urdf2dh report: " << endl;
for(size_t i=0; i < undirected_tree.getNrOfLinks(); i++ )
{
std::string frame= undirected_tree.getLink(i)->getName();
cerr << frame << " as base frame : " << endl;
cerr << "\t total frame couples failed : " << failure_as_base_frame[i] << endl;
cerr << frame << " as ee frame : " << endl;
cerr << "\t total frame couples failed : " << failure_as_ee_frame[i] << endl;
}
cerr << "total frame couples tested : " << failure_count + success_count << endl;
cerr << "total frame couples failed : " << failure_count << endl;
cerr << "total frame couples success : " << success_count << endl;
return test_success;
}
