//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;
 }
Beispiel #2
0
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)));
    }
}
Beispiel #3
0
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++;
        }
    }
}