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