//code modified from bool Tree::addTreeRecursive(..)
bool TreeInertialParameters::changeInertialParametersRecursive(const Eigen::VectorXd & new_chain_param, Tree & new_tree, SegmentMap::const_iterator root, const std::string& hook_name)
{
//Working segment object
Segment seg;
//get iterator for root-segment
SegmentMap::const_iterator child;
//try to add all of root's children
for (unsigned int i = 0; i < root->second.children.size(); i++) {
child = root->second.children[i];
//Modify segment
seg = child->second.segment;
seg.setInertia(deVectorize(tree_param.segment(serial.getLinkId(child->first)*10,10)));
//Try to add the modified child
if (new_tree.addSegment(seg, hook_name)) {
//if child is added, add all the child's children
if (!(this->changeInertialParametersRecursive(new_chain_param,new_tree,child, child->first)))
//if it didn't work, return false
return false;
} else
//If the child could not be added, return false
return false;
}
return true;
}
void inertialParametersVectorToUndirectedTreeLoop(const Eigen::VectorXd & parameters_vector,
UndirectedTree & undirected_tree)
{
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
undirected_tree.getLink(i,false)->setInertia(deVectorize(parameters_vector.segment(i*10,10)));
}
}
void inertialParametersVectorToUndirectedTreeLoopFakeLinks(
const Eigen::VectorXd & parameters_vector,
UndirectedTree & undirected_tree,
std::vector< std::string > fake_links_names)
{
int real_index_loop = 0;
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
if( std::find(fake_links_names.begin(), fake_links_names.end(), undirected_tree.getLink(i)->getName()) == fake_links_names.end() ) {
undirected_tree.getLink(i,false)->setInertia(deVectorize(parameters_vector.segment(real_index_loop*10,10)));
real_index_loop++;
}
}
}