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;
}
Esempio n. 2
0
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);
}