KinematicTree.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Micha Hersch, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #include "KinematicTree.h"
00020 
00021 #define STAND_UP
00022 ArticulatedTree::ArticulatedTree(){
00023   parent=NULL;
00024   children.clear();
00025   // inverse = 1;
00026 }
00027 
00028 #ifndef NO_CSOLID
00029 ArticulatedTree::ArticulatedTree(const pCSolidTree_t stree){
00030   parent = NULL;
00031   children.clear();
00032   //  inverse=1;
00033   joint.RotFromMatrix(stree->m_ref.m_orient);
00034   joint.SetTranslation(stree->m_ref.m_origin);
00035   pArticulatedTree_t next;
00036   CSolidTreeList_t::const_iterator it;
00037   for(it=stree->m_next.begin(); it!=stree->m_next.end(); ++it){
00038     next = new ArticulatedTree(*it);
00039     children.push_back(next);
00040     next->parent= this;
00041   }
00042 }
00043 
00044 #endif
00045 
00046 
00047 ArticulatedTree::ArticulatedTree(const pTree config){
00048   unsigned int i,j;
00049   float f;
00050   CVector3_t v;
00051   parent = NULL;
00052   children.clear(); 
00053 
00054 
00055   name = config->GetData();
00056   //  cout<<"adding "<<name<<endl;
00057   Tree_List *subTrees = config->GetSubTrees();
00058   //  cout<<"sub "<<subTrees->size()<<endl;
00059   for(i=0;i<(*subTrees).size();i++){
00060     if((*subTrees)[i]->GetName().compare("Axis")==0){
00061       istringstream s((*subTrees)[i]->GetData());
00062       s >> v[0]>>v[1]>>v[2];
00063       joint.SetRotationAxis(v);
00064     }
00065     if((*subTrees)[i]->GetName().compare("Angle")==0){
00066       istringstream s((*subTrees)[i]->GetData());
00067       s >> f;
00068       joint.SetAngle(f*deg2rad);
00069     }
00070     if((*subTrees)[i]->GetName().compare("Position")==0){
00071       istringstream s((*subTrees)[i]->GetData());
00072       s >> v[0]>>v[1]>>v[2];
00073       joint.SetTranslation(v);
00074     }
00075 #ifdef WITH_RANGES
00076     if((*subTrees)[i]->GetName().compare("Range")==0){
00077       istringstream s((*subTrees)[i]->GetData());
00078       s>>range[0]>>range[1];
00079       range[0] *= deg2rad;
00080       range[1] *= deg2rad;
00081       //      cout<<range[0]<<" "<<range[1]<<endl;
00082     }
00083 #endif
00084     if((*subTrees)[i]->GetName().compare("Children")==0){
00085       Tree_List *children = (*subTrees)[i]->GetSubTrees();
00086       //      cout<<" children "<<children->size()<<endl;
00087       for(j=0;j<children->size();j++){
00088         AddTree(new ArticulatedTree((*children)[j]));
00089       }
00090     }
00091   }
00092 } 
00093 
00094 ArticulatedTree::ArticulatedTree(const pArticulatedTree_t tree){
00095   ArticulatedTreeList_t::const_iterator it;
00096   joint = tree->joint;
00097   //  inverse = tree->inverse;
00098   name = tree->name;
00099   for(it=tree->children.begin(); it!=tree->children.end(); ++it){
00100     AddTree(new ArticulatedTree(*it));
00101   }
00102 }
00103 ArticulatedTree::~ArticulatedTree(){
00104   ArticulatedTreeList_t::const_iterator it; 
00105   for(it=children.begin(); it!=children.end(); ++it){
00106     delete (*it);
00107   }
00108 }
00109 
00110 void ArticulatedTree::AddTree(pArticulatedTree_t ptree){
00111  ptree->parent = this; 
00112  children.push_back(ptree);
00113 }
00114 
00115 pArticulatedTree_t ArticulatedTree::FindJoint(string& s){
00116   if(name.compare(s)==0){
00117     return this;
00118   }
00119   else{
00120     ArticulatedTreeList_t::const_iterator it;
00121     pArticulatedTree_t res;
00122     for(it=children.begin(); it!=children.end(); ++it){
00123       res =(*it)->FindJoint(s);
00124       if(res != NULL){ 
00125         return res;
00126       }
00127     }
00128     return NULL;
00129   }
00130 }
00131 
00139 pArticulatedTree_t ArticulatedTree::FindSubTree(pArticulatedTree_t t, pArticulatedTree_t exclude){
00140   if(this==t){
00141     return this;
00142   }
00143   else{
00144     ArticulatedTreeList_t::const_iterator it;
00145     pArticulatedTree_t res;
00146     for(it=children.begin(); it!=children.end(); ++it){
00147       if(*it != exclude){
00148         res =(*it)->FindSubTree(t,exclude);
00149         if(res != NULL){ 
00150           return res;
00151         }
00152       }
00153     }
00154     return NULL;
00155   }
00156 }
00157 
00158 
00159 void ArticulatedTree::RandomAxis(){
00160   ArticulatedTreeList_t::const_iterator it;
00161   joint.RandomAxis();
00162   for(it=children.begin(); it!=children.end(); ++it){
00163     (*it)->RandomAxis();
00164   }
00165 }
00166 
00167 void ArticulatedTree::RandomAngle(){
00168   ArticulatedTreeList_t::const_iterator it;
00169 #ifdef WITH_RANGES
00170   float angle = RND(range[1]-range[0])+range[0];
00171   joint.SetAngle(angle);
00172 #else
00173   joint.RandomAngle();
00174 #endif
00175   for(it=children.begin(); it!=children.end(); ++it){
00176     (*it)->RandomAngle();
00177   }  
00178 }
00179 void ArticulatedTree::ZeroPosition(){
00180   ArticulatedTreeList_t::const_iterator it;
00181   joint.SetAngle(0);
00182 #ifdef STAND_UP
00183     if(name=="r_knee"){
00184       joint.SetAngle(-pi/2);
00185     } else{  
00186       if(name =="l_knee"){
00187         joint.SetAngle(pi/2);
00188       }
00189     }
00190 #endif
00191   for(it=children.begin(); it!=children.end(); ++it){
00192     (*it)->ZeroPosition();
00193   }
00194 }
00195 
00196 
00197 
00198 void ArticulatedTree::NoTranslation(){
00199   ArticulatedTreeList_t::const_iterator it;
00200   CVector3_t v;
00201   v_set(RND(0.01)-0.005,RND(0.01)-0.005,RND(0.01)-0.005,v)
00202   joint.SetTranslation(v);
00203   for(it=children.begin(); it!=children.end(); ++it){
00204     (*it)->NoTranslation();
00205   }
00206 }
00207 
00213 void ArticulatedTree::CopyTree(pArticulatedTree_t tree){
00214   unsigned int j;
00215   CVector3_t v;
00216   tree->GetAxis(v);
00217   joint.SetRotationAxis(v);
00218   //  tree->GetJoint()->GetRotationParam(v);
00219   //  joint.SetTransfo(v);
00220   joint.SetTranslation(tree->GetJoint()->GetTranslation());
00221   for(j=0;j<children.size();j++){
00222     children[j]->CopyTree(tree->GetChildren()->at(j));
00223   } 
00224 }
00225 
00226 
00227 void ArticulatedTree::StreamTree(ostream& out) const {
00228   CVector3_t v;
00229   // int tmp = inverse;
00230   //  Invert(1);
00231   out<<"<Segment> "<<name<<endl;
00232   joint.GetRotationAxis(v);
00233   out<<"<Axis> "<<v<<" </Axis>"<<endl;
00234   out<<"<Angle> "<<joint.GetAngle()*rad2deg<<" </Angle>"<<endl;
00235   joint.GetTranslation(v);
00236   out<<"<Position> "<<v<<" </Position>"<<endl;
00237   //  Invert(tmp);
00238   if(children.size()>0){
00239     out<<"<Children> "<<endl;
00240     ArticulatedTreeList_t::const_iterator it; 
00241    for(it=children.begin(); it!=children.end(); ++it){
00242       (*it)->StreamTree(out);
00243     }
00244     out<<"</Children>"<<endl;
00245   } 
00246  out<<"</Segment>"<<endl;
00247 }
00248 
00249 
00250 void ArticulatedTree::Reshape(CVector3_t offset){//make use of v
00251   CVector3_t ax,v1,tr;
00252   float norm,vn;
00253   if(parent){
00254     joint.GetRotationAxis(ax);
00255     joint.GetTranslation(tr);
00256     if(offset){
00257       v_add(tr,offset,tr);
00258     }
00259     vn = v_normalize(tr,v1);
00260     norm = v_dot(v1,ax);
00261     v_scale(ax,norm*vn,v1);
00262     v_sub(tr,v1,ax);
00263     //    cout<<"reshape "<<v1<<endl;
00264     joint.SetTranslation(ax);
00265   }
00266   ArticulatedTreeList_t::const_iterator it; 
00267   for(it=children.begin(); it!=children.end(); ++it){
00268     (*it)->Reshape(v1);
00269   }
00270 }
00271 
00272 void ArticulatedTree::Reshape2(CVector3_t v1){//make use of v
00273   CVector3_t ax,tr,v;
00274   float norm,vn;
00275   ArticulatedTreeList_t::const_iterator it; 
00276   for(it=children.begin(); it!=children.end(); ++it){
00277     (*it)->Reshape2(v);
00278     
00279   if(parent){
00280     parent->GetAxis(ax);
00281     joint.GetTranslation(tr);
00282     v_add(tr,v,tr);
00283     vn = v_normalize(tr,tr);
00284     norm = v_dot(tr,ax);
00285     v_scale(ax,norm*vn,v1);
00286     v_sub(tr,v1,ax);
00287     //    cout<<"reshape "<<v1<<endl;
00288     joint.SetTranslation(ax);
00289   }
00290   }
00291 
00292 }
00293 
00294 #ifndef NO_CSOLID
00295 
00296 pCSolidTree_t ArticulatedTree::CreateCSolidTreeStruct(){
00297   pCSolidTree_t st = new CSolidTree();
00298   if(children.size()>0){
00299     ArticulatedTreeList_t::const_iterator it; 
00300     for(it=children.begin(); it!=children.end(); ++it){
00301       st->AddSolid((*it)->CreateCSolidTreeStruct());
00302     }
00303   }
00304   return st;
00305 }
00306 
00307 
00308 void ArticulatedTree::FillCSolidTree(pCSolidTree_t solid){
00309 
00310 //   if(parent){ 
00311 //       //          GetParent()->GetJoint()->GetTranslation(solid->m_ref.m_origin);
00312 //   GetParent()->GetJoint()->RotToMatrix(solid->m_ref.m_orient);
00313 //   } 
00314   joint.GetTranslation(solid->m_ref.m_origin);   
00315   joint.RotToMatrix(solid->m_ref.m_orient);
00316     solid->m_name = name;
00317     joint.GetRotationAxis(solid->m_position);//rotation axis is put in m_position var
00318   // joint.GetTranslation(v);
00319 //   solid->m_size);
00320 //   v_scale(solid->m_size,-0.5,solid->m_position);
00321 
00322   // v_length(v);
00323   // v_set(10,20,30,solid->m_size);
00324   for(unsigned int i=0; i<children.size();i++){
00325     children[i]->FillCSolidTree(solid->m_next[i]);
00326   }
00327 }
00328   
00329 #endif
00330 
00331 
00339 int ArticulatedTree::CheckPairsOfNodes(int (*f)(pArticulatedTree_t, pArticulatedTree_t, void*), void *arg){
00340   unsigned int i,j;
00341   int nb=0;
00342   //nb = (*f)(this,this,arg); //if one wants pairs of same elements
00343    for(i=0; i<children.size();i++){ 
00344      nb+=children[i]->CheckPairsOfNodes(f,arg); // the pair of nodes is in the same subtree i
00345      nb+=children[i]->CheckTreeNode1(this,f,arg); // the pairs composed of this and all descent of i
00346      for(j=i+1; j<children.size();j++){           // the pairs composed of one elmnt in i and one in j 
00347        nb+= children[j]->CheckTreeNode2(children[i],f,arg);
00348      }
00349    }
00350    return nb;
00351 }
00352 
00353 int ArticulatedTree::CheckTreeNode1(pArticulatedTree_t tree, int (*f)(pArticulatedTree_t, pArticulatedTree_t,void*), void* arg){
00354     ArticulatedTreeList_t::const_iterator it;
00355     int nb=0;
00356     nb = (*f)(this,tree,arg);
00357     for(it=children.begin(); it!=children.end(); ++it){
00358       nb+=(*it)->CheckTreeNode1(tree,f,arg);
00359     }
00360     return nb;
00361 }
00362 
00363 int ArticulatedTree::CheckTreeNode2(pArticulatedTree_t tree, int (*f)(pArticulatedTree_t, pArticulatedTree_t,void*), void* arg){
00364     ArticulatedTreeList_t::const_iterator it;
00365     int nb=0;
00366     nb+=CheckTreeNode1(tree,f,arg);
00367     for(it=tree->GetChildren()->begin(); it!=tree->GetChildren()->end(); ++it){
00368       nb+=CheckTreeNode2(*it,f,arg);
00369     }
00370  
00371     return nb;
00372 }
00373 
00374 
00375 int ArticulatedTree::Serialize(float *data,int data_size)const{
00376     int n=1; 
00377     ArticulatedTreeList_t::const_iterator it;
00378     if(data_size<6){
00379         cout<<"warning: writing outside buffer in ArticulatedTree::Serialize"
00380             <<endl;
00381         return 0;
00382     }
00383     v_copy(joint.GetTranslation(),data);
00384     v_copy(joint.GetRotationAxis(),data+3);
00385     for(it=children.begin(); it!=children.end(); ++it){
00386         n+=(*it)->Serialize(data+n*6,data_size-n*6);
00387         
00388     }
00389     return n;
00390 }
00391 
00392 int ArticulatedTree::Deserialize(const float *data,int data_size){
00393 
00394     int n=1; 
00395     ArticulatedTreeList_t::const_iterator it;
00396     if(data_size<6){
00397         cout<<"warning: writing outside buffer in ArticulatedTree::Deserialize"
00398             <<endl;
00399         return 0;
00400     }
00401     joint.SetTranslation(data);
00402     joint.SetRotationAxis(data+3);
00403     for(it=children.begin(); it!=children.end(); ++it){
00404         n+=(*it)->Deserialize(data+n*6,data_size-n*6);
00405         
00406     }
00407     return n;
00408 }
00409 
00410 
00411 int ArticulatedTree::GetAngleList(float *list){
00412   ArticulatedTreeList_t::const_iterator it;
00413   int n=1;
00414   *list= joint.GetAngle();
00415   for(it=children.begin(); it!=children.end(); ++it){
00416     n+=(*it)->GetAngleList(list+n);
00417   }
00418   return n;
00419 }
00420 
00421 
00422 #ifdef WITH_RANGES
00423 int ArticulatedTree::GetAngleRangeList(float *list_min,float *list_max){
00424   ArticulatedTreeList_t::const_iterator it;
00425   int n=1;
00426   *list_min=range[0];
00427   *list_max=range[1];
00428   for(it=children.begin(); it!=children.end(); ++it){
00429     n+=(*it)->GetAngleRangeList(list_min+n,list_max+n);
00430   }
00431   return n;
00432 }
00433 
00434 //lower_range and upper_range are assumed to have room for the right number of elements
00435 int ArticulatedTree::FindAngleRanges(float *lower_range, float *upper_range,
00436                                      KinematicChain *chain){
00437     ArticulatedTreeList_t::const_iterator it;
00438     int i,n,k=0;
00439     n = chain->GetNbJoints();
00440     for(i=0;i<n;i++){
00441         if(&joint == chain->GetTransfo(i)){
00442             lower_range[i] = range[0];
00443             upper_range[i] = range[1];
00444             k=1;
00445             break;
00446         }
00447     }
00448     for(it=children.begin(); it!=children.end(); ++it){
00449         if(k==n) return k;
00450         k+=(*it)->FindAngleRanges(lower_range,upper_range,chain);
00451     }
00452     return k;
00453 }
00454 
00455 #endif
00456 
00457 
00458 int ArticulatedTree::SetAngleList(float *list){
00459   ArticulatedTreeList_t::const_iterator it;
00460   int n=1;
00461   joint.SetAngle(*list);
00462   for(it=children.begin(); it!=children.end(); ++it){
00463     n+=(*it)->SetAngleList(list+n);
00464   }
00465   return n;
00466 }
00467 
00468 int ArticulatedTree::GetSize()const{
00469   ArticulatedTreeList_t::const_iterator it;
00470   int n=1;
00471   for(it=children.begin(); it!=children.end(); ++it){
00472     n+=(*it)->GetSize();
00473   }
00474   return n;
00475 }
00476 
00477 void ArticulatedTree::PrintAngles(){
00478   ArticulatedTreeList_t::const_iterator it;
00479   cout<<joint.GetAngle()<<" ";
00480   for(it=children.begin(); it!=children.end(); ++it){
00481     (*it)->PrintAngles();
00482   }
00483 }
00484 
00485 
00486 void ArticulatedTree::PrintList(){
00487   ArticulatedTreeList_t::const_iterator it;
00488   cout<<name<<endl;
00489   for(it=children.begin(); it!=children.end(); ++it){
00490     (*it)->PrintList();
00491   }
00492 }
00493 
00494 ostream& operator<<(ostream& out, const ArticulatedTree& at){
00495   at.StreamTree(out);
00496   return out;
00497 }
00498 
00499 KinematicTree::KinematicTree(){
00500   xml_tree = NULL;
00501   root = NULL;
00502   solid = NULL;
00503   nb_chains=0;
00504 }
00505 
00506 KinematicTree::KinematicTree(const char *filename){
00507   xml_tree = new Tree();
00508   solid = NULL;
00509   xml_tree->LoadFromFile(string(filename));
00510   root = new ArticulatedTree(xml_tree);
00511   //solid = root->CreateCSolidTreeStruct();
00512 }
00513 
00514 // dangerous for memory deallocation
00515 // KinematicTree::KinemtaticTree(pArticulatedTree_t atree){
00516 //   solid=NULL;
00517 //   xml_tree = NULL:
00518 //   root = atree;
00519 // }
00520 
00521 
00522 KinematicTree::~KinematicTree(){
00523   if(xml_tree)delete xml_tree;
00524   if(root) delete root;
00525 #ifndef NO_CSOLID
00526   if(solid)delete solid;
00527 #endif
00528 }
00529 
00530 
00531 
00537 #ifdef WITH_LAST_LINK
00538 
00544 int KinematicTree::LoadChain(pArticulatedTree_t from, pArticulatedTree_t to,
00545                              KinematicChain *kc, modality_t mod)const{
00546 
00547  
00548     kc->Clear();
00549     kc->SetModality(mod);
00550     //    kc->AddLastLink(to->GetJoint());
00551     int ret= LoadChainRec(from,to,kc);
00552     if(ret){
00553         if(kc->IsInverted(ret-1)==1){//not inverted
00554             kc->AddLastLink(kc->GetTransfo(ret-1));
00555             kc->RemoveJoint();
00556             return ret-1;
00557         }
00558     }
00559     return ret;
00560 }
00561 
00562 #else
00563 
00564 
00565 
00566 int KinematicTree::LoadChain(pArticulatedTree_t from, pArticulatedTree_t to, KinematicChain *kc,
00567                              modality_t mod)const{
00568   kc->Clear();
00569   kc->SetModality(mod);
00570   //  cout<<"done"<<endl;
00571   return LoadChainRec(from,to,kc);
00572 }
00573 
00574 #endif
00575 
00576 
00577 inline int KinematicTree::LoadChain(pArticulatedTree_t from, pArticulatedTree_t to, int i,
00578                                      modality_t mod){
00579   return LoadChain(from,to,&(chains[i]),mod);
00580 }
00581 
00582 
00592 int KinematicTree::LoadChainRec(pArticulatedTree_t from, pArticulatedTree_t to, 
00593                                 KinematicChain *chain,pArticulatedTree_t exclude)const{
00594   if(to==from){
00595       if(chain->GetNbJoints()){ //the chain is on two branches - 
00596           return chain->GetNbJoints();
00597       }
00598       else{//the chain is on the same branch
00599           cout<<"adding "<<to->name<<endl;
00600           return chain->AddJoint(to->GetJoint(),1);
00601       }     
00602   }
00603   // from is not above to
00604   if(from->FindSubTree(to,exclude)==NULL){
00605     chain->AddJoint(from->GetJoint(),-1);
00606     cout<<"adding "<<from->name<<endl;
00607     if(from->GetParent()==to){
00608         cout<<"adding "<<to->name<<endl;
00609         return chain->AddJoint(to->GetJoint(),-1);
00610     }
00611     return LoadChainRec(from->GetParent(),to,chain,from);
00612   }
00613   LoadChainRec(from,to->GetParent(),chain);
00614   cout<<"adding "<<to->name<<endl;
00615   return chain->AddJoint(to->GetJoint(),1);
00616 }
00617 
00618 
00619  int KinematicTree::LoadChain(string& from, string& to, int i, modality_t mod){
00620   return LoadChain(from,to,&(chains[i]),mod);
00621 }
00622 int KinematicTree::LoadChain(string& from, string& to, KinematicChain *kc, modality_t mod){
00623   pArticulatedTree_t f, t;
00624   f=root->FindJoint(from); 
00625   t=root->FindJoint(to);
00626   if(f && t){
00627     return LoadChain(f,t,kc,mod);
00628   }
00629   else{
00630     return -1;
00631   }
00632 }
00633 
00634 KinematicChain * KinematicTree::GetNewChain(string& from, string& to,modality_t mod){
00635   KinematicChain *kc = new KinematicChain();
00636   if(LoadChain(from,to,kc,mod)>0){
00637     return kc;
00638   }
00639   else{
00640     return NULL;
00641   }
00642 }
00643 
00656 int KinematicTree::CollisionDetection(pArticulatedTree_t t1, pArticulatedTree_t t2, void *arg){
00657   collision_arg_t *state;
00658   // CVector3_t v,v2,p1,p2;
00659   float k1,k2,distance=-1.0f;
00660   state = (collision_arg_t *) arg;
00661   //  KinematicChain kc;
00662   //  KinematicTree kt;
00663   //  RigidTransfo& rt = state->rt;
00664   if(t1->GetParent()==t2 || t2->GetParent()==t1){return 0;}
00665   if(t1->GetParent()==state->t1 && t2 == state->t2 && 0) {// and t2 is not a subtree of t1 -> just add a transformation  for optimizing processing speed
00666    //  rt = t1->GetJoint();
00667 //     state->kc->AddJoint(rt,-1);
00668 //     //  rt->InverseTransform(state->pos_stack[state->top],state->pos_stack[state->top-1]); 
00669 //     //  state->top--;
00670 //     rt = rt
00671 //     state->t1 = t1;
00672   }
00673   else{
00674     state->kt->LoadChain(t1,t2,state->kc, TOUCH_MODALITY);   
00675 }
00676 
00677    distance = state->kc->CollisionDetection(k1,k2);
00678   if(distance<0){
00679     return 0;
00680   }
00681   if(distance<=state->dist){
00682     //    cout<<t1->name<<" "<<t2->name<<" "<<distance<<endl;
00683     if(state->kt_update){
00684       float angles[MAX_LINKS];
00685       state->kc->GetAngles(angles);
00686        state->kt_update->LoadChain(t1->name,t2->name,&(state->kc_update),TOUCH_MODALITY);      
00687        state->kc_update.UpdateTouch(angles,k1,k2);
00688     }    
00689     return 1;
00690   }
00691   else{
00692     return 0;
00693   }
00694 }
00695  
00696 //static collision_arg_t arg;
00697 
00698 int KinematicTree::CheckAllCollisions(KinematicTree *kt2=NULL){
00699   collision_arg_t arg;
00700   arg.dist = 10; 
00701   arg.kt_update = kt2;
00702   arg.kt = this;
00703   arg.kc = new KinematicChain();
00704   return root->CheckPairsOfNodes(KinematicTree::CollisionDetection,&arg);
00705 }
00706 
00707 // KinematicTree::Render(){
00708 //   if(!solid){
00709 //     if(root){
00710 //       solid = root->CreateCSolidTreeStruct();
00711 //     }
00712 //   }
00713   
00714 // }
00715 
00716 
00717 
00718 
00719 
00720 //#define TEST_TREES2
00721 
00722 #ifdef TEST_TREES
00723 
00724 
00725 int main(int argc, char *argv[]){
00726   cout<<"loading tree"<<endl;
00727   CVector3_t pos;
00728   float angles[MAX_LINKS];
00729   string s1("head");
00730   string s2("r_wrist");
00731   string s3("r_elbow"),s4(" ");
00732   pArticulatedTree_t at2;
00733   KinematicChain *c1,*c2;
00734   KinematicTree *kt1 = new KinematicTree(argv[1]);
00735   KinematicTree *kt2 = new KinematicTree(argv[1]);
00736   at2 =kt2->GetArticulatedTree();
00737   //  at2->RandomAxis();
00738   at2->NoTranslation();
00739   kt1->LoadChain(s1,s2,0);
00740   kt2->LoadChain(s1,s2,0);
00741   c1 = kt1->GetChain(0);
00742   c2 = kt2->GetChain(0);
00743   for(int i=0;i<100000;i++){
00744     if(rand()%100==0){
00745       s4 = s2;
00746       s2 = s3;
00747       s3 = s4;
00748 
00749       kt1->LoadChain(s1,s2,0);
00750       kt2->LoadChain(s1,s2,0);
00751       c1 = kt1->GetChain(0);
00752       c2 = kt2->GetChain(0);
00753     }
00754 
00755     c1->RandomAngles(angles);
00756     c1->ForwardKinematics(angles,pos);
00757     c2->Update(angles,pos);
00758   }
00759   
00760   at2->StreamTree(cout);
00761 //   kt->LoadChain(s1,s2,0);
00762 //   cout<<"n joints: "<<kt->GetChain(0)->GetNbJoints()<<endl;
00763 //   for(int i=0;i<kt->GetChain(0)->GetNbJoints();i++){
00764 //     cout<<kt->GetChain(0)->Get
00765   
00766 
00767 
00768 
00769 #ifdef GRAPHICAL
00770   cout<<"drawing"<<endl;
00771   DrawBodyApplication app(argc,argv);
00772   app.Init();
00773   app.SetBody(st);
00774   app.Run();
00775 #endif
00776  cout<<"done";
00777   return 0;
00778 }
00779 #endif
00780 
00781 
00782 #ifdef TEST_TREES2
00783 // int func(pArticulatedTree_t t1,pArticulatedTree_t t2,void *arg){
00784 //   //  cout<<t1->name<<" : "<<t2->name<<endl;
00785 //   KinematicTree kt;
00786 //   return kt.CollisionDetection(10,t1,t2,arg);
00787 // }
00788 
00789 
00790 int main(int argc, char *argv[]){
00791    pArticulatedTree_t at;
00792    int n=0;
00793    float l1,l2,dist;
00794    collision_arg_t arg;
00795    arg.dist = 10;
00796    KinematicTree *kt = new KinematicTree(argv[1]);
00797    at =kt->GetArticulatedTree();
00798    srand(time(NULL));
00799    do{
00800      at->RandomAngle();
00801      at->GetJoint()->SetAngle(0);
00802      n++;
00803    } while(!at->CheckPairsOfNodes(KinematicTree::CollisionDetection,&arg));
00804    cout<<"nb it "<<n<<endl;
00805    cout<<*at<<endl;
00806 
00807 //    KinematicChain kc;
00808 //    //   at->RandomAngle();
00809 //    kt->LoadChain("r_sfe","r_wrist",&kc);
00810 //    dist = kc.CollisionDetection(l1,l2);
00811 //    cout<<"res "<<dist<<" "<<l1<<" "<<l2<<endl;
00812 //    kt->LoadChain("r_wrist","r_sfe",&kc);
00813 //    dist = kc.CollisionDetection(l1,l2);
00814 //    cout<<"res "<<dist<<" "<<l1<<" "<<l2<<endl;
00815    
00816    return n;
00817 }
00818 
00819 
00820 #endif
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:26 2010 for Body_Schema_Learning by  doxygen 1.6.1