bool ExternalWrenchesAndTorquesEstimator::init()
{
    resizeAll(sensors->getSensorNumber(SENSOR_ENCODER));
    resizeFTs(sensors->getSensorNumber(SENSOR_FORCE_TORQUE));
    resizeIMUs(sensors->getSensorNumber(SENSOR_IMU));

    //Allocation model
    if(  !wbi_yarp_conf.check("urdf") && !wbi_yarp_conf.check("urdf_file") )
    {
        std::cerr << "yarpWholeBodyModel error: urdf not found in configuration files" << std::endl;
        return false;
    }

    std::string urdf_file;
    if( wbi_yarp_conf.check("urdf") )
    {
        urdf_file = wbi_yarp_conf.find("urdf").asString().c_str();
    }
    else
    {
        urdf_file = wbi_yarp_conf.find("urdf_file").asString().c_str();
    }

    yarp::os::ResourceFinder rf;
    std::string urdf_file_path = rf.findFileByName(urdf_file.c_str());

    std::vector<std::string> dof_serialization;
    IDList torque_estimation_list = sensors->getSensorList(SENSOR_ENCODER);
    for(int dof=0; dof < (int)torque_estimation_list.size(); dof++)
    {
        ID wbi_id;
        torque_estimation_list.indexToID(dof,wbi_id);
        dof_serialization.push_back(wbi_id.toString());
    }

    std::vector<std::string> ft_serialization;
    IDList ft_sensor_list = sensors->getSensorList(SENSOR_FORCE_TORQUE);
    for(int ft=0; ft < (int)ft_sensor_list.size(); ft++)
    {
        ID wbi_id;
        ft_sensor_list.indexToID(ft,wbi_id);
        ft_serialization.push_back(wbi_id.toString());
    }

    {
        std::cerr << "[DEBUG] Create TorqueEstimationTree with " << ft_serialization.size() << " ft sensors" << std::endl;
        if( !assume_fixed_base )
        {
            robot_estimation_model = new iCub::iDynTree::TorqueEstimationTree(urdf_file_path,dof_serialization,ft_serialization);
        } else {
            robot_estimation_model = new iCub::iDynTree::TorqueEstimationTree(urdf_file_path,dof_serialization,ft_serialization,fixed_link);
        }

        if( this->assume_fixed_base_from_odometry )
        {
            robot_estimation_model_on_l_sole = new iCub::iDynTree::TorqueEstimationTree(urdf_file_path,dof_serialization,ft_serialization,"l_sole");
            robot_estimation_model_on_r_sole = new iCub::iDynTree::TorqueEstimationTree(urdf_file_path,dof_serialization,ft_serialization,"r_sole");
        }
    }
    //Load mapping from skinDynLib to iDynTree links from configuration files

    if( !this->wbi_yarp_conf.check("IDYNTREE_SKINDYNLIB_LINKS") )
    {
        std::cerr << "[ERR] wholeBodyDynamicsStatesInterface error: IDYNTREE_SKINDYNLIB_LINKS group not found in configuration files" << std::endl;
        return false;
    }

    yarp::os::Bottle & bot =  this->wbi_yarp_conf.findGroup("IDYNTREE_SKINDYNLIB_LINKS");
    for(int i=1; i < bot.size(); i++ )
    {
        yarp::os::Bottle * map_bot = bot.get(i).asList();
        if( map_bot->size() != 2 || map_bot->get(1).asList() == NULL ||
            map_bot->get(1).asList()->size() != 3 )
        {
            std::cerr << "[ERR] wholeBodyDynamicsStatesInterface error: IDYNTREE_SKINDYNLIB_LINKS group is malformed (" << map_bot->toString() << ")" << std::endl;
            return false;
        }
        std::string iDynTree_link_name = map_bot->get(0).asString();
        std::string iDynTree_skinFrame_name = map_bot->get(1).asList()->get(0).asString();
        int skinDynLib_body_part = map_bot->get(1).asList()->get(1).asInt();
        int skinDynLib_link_index = map_bot->get(1).asList()->get(2).asInt();
        bool ret_sdl = robot_estimation_model->addSkinDynLibAlias(iDynTree_link_name,iDynTree_skinFrame_name,skinDynLib_body_part,skinDynLib_link_index);
        if( this->assume_fixed_base_from_odometry )
        {
            robot_estimation_model_on_l_sole->addSkinDynLibAlias(iDynTree_link_name,iDynTree_skinFrame_name,skinDynLib_body_part,skinDynLib_link_index);
            robot_estimation_model_on_r_sole->addSkinDynLibAlias(iDynTree_link_name,iDynTree_skinFrame_name,skinDynLib_body_part,skinDynLib_link_index);
        }

        if( !ret_sdl )
        {
            std::cerr << "[ERR] wholeBodyDynamicsStatesInterface error: IDYNTREE_SKINDYNLIB_LINKS link " << iDynTree_link_name
                      << " and frame " << iDynTree_skinFrame_name << " and not found in urdf model" << std::endl;
            return false;
        }
    }
    std::cerr << std::endl << "[INFO] IDYNTREE_SKINDYNLIB_LINKS correctly loaded" << std::endl;


    //Load subtree information
    link2subtree.resize(robot_estimation_model->getNrOfLinks());

    if( !this->wbi_yarp_conf.check("WBD_SUBTREES") )
    {
        std::cerr << "[ERR] wholeBodyDynamicsStatesInterface error: WBD_SUBTREES group not found in configuration files" << std::endl;
        return false;
    }

    yarp::os::Bottle & wbd_subtrees_bot =  this->wbi_yarp_conf.findGroup("WBD_SUBTREES");


    for(int i=1; i < wbd_subtrees_bot.size(); i++ )
    {
        yarp::os::Bottle * subtree_bot = wbd_subtrees_bot.get(i).asList();
        if( subtree_bot->size() != 2
            || subtree_bot->get(1).asList() == NULL
            || subtree_bot->get(1).asList()->size() != 2
            || subtree_bot->get(1).asList()->get(0).asList() == NULL )
        {
            std::cerr << "[ERR] wholeBodyDynamicsStatesInterface error: WBD_SUBTREES group is malformed (" << subtree_bot->toString() << ")" << std::endl;
            return false;
        }


        TorqueEstimationSubtree subtree;

        std::string subtree_name = subtree_bot->get(0).asString();
        subtree.subtree_name = subtree_name;


        yarp::os::Bottle * subtree_links_bot = subtree_bot->get(1).asList()->get(0).asList();
        for(int l=0; l < subtree_links_bot->size(); l++ )
        {

            std::string link_name = subtree_links_bot->get(l).asString();
            int link_index = robot_estimation_model->getLinkIndex(link_name);
            if( link_index < 0 )
            {
                std::cerr << "[ERR] wholeBodyDynamicsStatesInterface error: WBD_SUBTREES link " << link_name << " not found in urdf model" << std::endl;
                return false;
            }
            subtree.links.push_back(link_name);
            subtree.links_numeric_ids.insert(link_index);

            int current_subtree = i-1;
            link2subtree[link_index] = current_subtree;

        }
        std::string default_contact_link_name = subtree_bot->get(1).asList()->get(1).asString();

        int default_contact_link_index = robot_estimation_model->getLinkIndex(default_contact_link_name);
        if( default_contact_link_index < 0 )
        {
            std::cerr << "[ERR] wholeBodyDynamicsStatesInterface error: WBD_SUBTREES link " << default_contact_link_name << " not found in urdf model" << std::endl;
            return false;
        }

        if( subtree.links_numeric_ids.find(default_contact_link_index) == subtree.links_numeric_ids.end() )
        {
            std::cerr << "[ERR] wholeBodyDynamicsStatesInterface error: WBD_SUBTREES link " << default_contact_link_name
                      << " was specified as default contact for subtree " << subtree.subtree_name
                      << " but it is not present in the subtree" << std::endl;
            return false;
        }

        subtree.default_contact_link = default_contact_link_index;

        torque_estimation_subtrees.push_back(subtree);
    }

    contacts_for_given_subtree.resize(torque_estimation_subtrees.size());

    std::cerr << "[DEBUG] robot_estimation_model->getSubTreeInternalDynamics().size() : " << robot_estimation_model->getSubTreeInternalDynamics().size() << std::endl;
    std::cerr << "[DEBUG] torque_estimation_subtrees.size(): " << torque_estimation_subtrees.size() << std::endl;
    yAssert(robot_estimation_model->getSubTreeInternalDynamics().size() ==  torque_estimation_subtrees.size());

    std::cerr << "[INFO] WBD_SUBTREES correctly loaded with " << torque_estimation_subtrees.size() << "subtrees" << std::endl;

    IDList available_encoders = sensors->getSensorList(wbi::SENSOR_ENCODER);
    for(int i = 0; i < (int)available_encoders.size(); i++ )
    {
        ID enc;
        available_encoders.indexToID(i,enc);
        if(!(robot_estimation_model->getDOFIndex(enc.toString()) == i))
        {
            std::cerr << "[ERR] dof " << enc.toString() << " has ID " << robot_estimation_model->getDOFIndex(enc.toString())
                      << " in the dynamical model and id " << i << "in the wbi" << std::endl;
        }
        yAssert((robot_estimation_model->getDOFIndex(enc.toString()) == i));
    }
    yDebug() << "ExternalWrenchesAndTorquesEstimator::init() terminated successfully";
    return true;
}