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