bool simpleLeggedOdometry::init(KDL::CoDyCo::UndirectedTree & undirected_tree,
const int initial_world_frame_position_index,
const int initial_fixed_link_index)
{
if( odometry_model )
{
delete odometry_model;
odometry_model=0;
}
odometry_model = new iCub::iDynTree::DynTree(undirected_tree.getTree(),undirected_tree.getSerialization());
bool ok = reset(initial_world_frame_position_index,initial_fixed_link_index);
return ok;
}
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);
}