EmbodiedArticulatedTree.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 
00020 #include "EmbodiedArticulatedTree.h"
00021 #include "EmbodiedKinematicChain.h"
00022 #include "GL/glut.h"
00023 
00024 
00025 
00026 EmbodiedArticulatedTree::EmbodiedArticulatedTree()
00027 :ArticulatedTree()
00028 {
00029   solid=NULL;
00030 }
00031 
00032 EmbodiedArticulatedTree::EmbodiedArticulatedTree(const pTree config){
00033  unsigned int i,j;
00034   float f;
00035   CVector3_t v;
00036   parent = NULL;
00037   solid=NULL;  
00038   children.clear(); 
00039   
00040   name = config->GetData();
00041  cout<<"adding "<<name<<endl;
00042   Tree_List *subTrees = config->GetSubTrees();
00043   //  cout<<"sub "<<subTrees->size()<<endl;
00044   for(i=0;i<(*subTrees).size();i++){
00045     if((*subTrees)[i]->GetName().compare("Axis")==0){
00046       istringstream s((*subTrees)[i]->GetData());
00047       s >> v[0]>>v[1]>>v[2];
00048         cout<<v<<endl;
00049       joint.SetRotationAxis(v);
00050     }
00051     if((*subTrees)[i]->GetName().compare("Angle")==0){
00052       istringstream s((*subTrees)[i]->GetData());
00053       s >> f;
00054       joint.SetAngle(f*deg2rad);
00055     }
00056 #ifdef WITH_RANGES
00057     if((*subTrees)[i]->GetName().compare("Range")==0){
00058       istringstream s((*subTrees)[i]->GetData());
00059       s>>range[0]>>range[1];
00060       range[0] *= deg2rad;
00061       range[1] *= deg2rad;
00062       //      cout<<range[0]<<" "<<range[1]<<endl;
00063     }
00064 #endif
00065     if((*subTrees)[i]->GetName().compare("Position")==0){
00066       istringstream s((*subTrees)[i]->GetData());
00067       s >> v[0]>>v[1]>>v[2];
00068       joint.SetTranslation(v);
00069     }
00070     if((*subTrees)[i]->GetName().compare("Children")==0){
00071       Tree_List *children = (*subTrees)[i]->GetSubTrees();
00072       //      cout<<" children "<<children->size()<<endl;
00073       for(j=0;j<children->size();j++){
00074         AddTree(new EmbodiedArticulatedTree((*children)[j]));
00075       }
00076     }
00077   }
00078 }
00079 
00080 
00081 EmbodiedArticulatedTree::~EmbodiedArticulatedTree(){
00082    if(solid)delete solid;
00083 }
00084 
00085 void EmbodiedArticulatedTree::RenderBegin(){
00086   CMatrix4_t mat;
00087   CVector3_t tr;
00088   joint.GetTranslation(tr);
00089   joint.RotToMatrix(mat);
00090   glPushMatrix();
00091   glTranslatef(tr[0],tr[1],tr[2]);
00092   glMultMatrixf(mat);
00093 }
00094 
00095 void EmbodiedArticulatedTree::RenderEnd(){
00096   glPopMatrix();
00097 }
00098 
00099 
00100 void EmbodiedArticulatedTree::Render(){
00101   RenderBegin();
00102   if(solid){
00103       solid->Render();
00104       solid->Unhighlight();
00105   }
00106    ArticulatedTreeList_t::const_iterator it; 
00107   for(it=children.begin(); it!=children.end(); ++it){
00108     ((EmbodiedArticulatedTree *)(*it))->Render();
00109   }
00110   RenderEnd();
00111 }
00112 
00113 void EmbodiedArticulatedTree::StreamTree(ostream& out) const {
00114  CVector3_t v;
00115  out<<"<Segment> "<<name<<endl;
00116  joint.GetRotationAxis(v);
00117  out<<"<Axis> "<<v<<" </Axis>"<<endl;
00118  out<<"<Angle> "<<joint.GetAngle()*rad2deg<<" </Angle>"<<endl;
00119  joint.GetTranslation(v);
00120  out<<"<Position> "<<v<<" </Position>"<<endl;
00121  //  Invert(tmp);
00122  if(solid){
00123    out<<"<Shape>"<<endl;
00124    solid->Stream(out);
00125    out<<"</Shape>"<<endl;
00126  }
00127  if(children.size()>0){
00128    out<<"<Children> "<<endl;
00129     ArticulatedTreeList_t::const_iterator it; 
00130    for(it=children.begin(); it!=children.end(); ++it){
00131       (*it)->StreamTree(out);
00132     }
00133     out<<"</Children>"<<endl;
00134   } 
00135  out<<"</Segment>"<<endl;
00136 
00137 }
00138 
00139 //initializes all the shape objects
00140 // file format
00141 // <Body>
00142 //   <Shape>
00143 //     <Name> r_eb </Name>
00144 //     <Type> Capsule </Type>
00145 //     <Params> 
00146 //        <Radius> 10 </Radius> 
00147 //        <Axis> 0 0 1 </Axis>
00148 //     </Params>
00149 //   </Shape>
00150 // </Body>  
00151 int EmbodiedArticulatedTree::SetAllShapes(pTree shape_tree){
00152   int i,n,j,nj,cnt=0;
00153   pArticulatedTree_t tree;
00154   Tree_List *shapes = shape_tree->GetSubTrees();
00155   string type;
00156   n= shapes->size();
00157   cout<<n<<" shapes investigated"<<endl;
00158   for(i=0;i<n;i++){
00159     if(shapes->at(i)->GetName().compare("Shape")==0){
00160       Tree_List *descr = shapes->at(i)->GetSubTrees();
00161       nj = descr->size();
00162       tree = NULL;
00163       type = "";
00164       pTree params =NULL;
00165       for(j=0;j<nj;j++){
00166         if(descr->at(j)->GetName().compare("Name")==0){
00167           string name = descr->at(j)->GetData();
00168           tree = FindJoint(name);
00169         }
00170         if(descr->at(j)->GetName().compare("Type")==0){
00171           type = descr->at(j)->GetData();
00172         }
00173         if(descr->at(j)->GetName().compare("Params")==0){
00174           params = descr->at(j);
00175         }
00176         if(tree && type!="" && params){
00177           if(!type.compare("Capsule")){
00178           if(!params->Find(string("Axis"))){
00179               // getting default capsule axis
00180               if(tree->GetChildren()->size()>0){
00181                   char tmp[80];
00182                   string axis_val;
00183                   CVector3_t axis;
00184                   tree->GetChildren()->at(0)->GetJoint()->GetTranslation(axis);
00185                   sprintf(tmp,"%.5f %.5f %.5f",axis[0],axis[1],axis[2]);
00186                   axis_val = tmp;
00187                   cout<<tmp<<endl;
00188                   params->AddSubTree(new Tree(string("Axis"),axis_val));
00189               }
00190           } 
00191       ((EmbodiedArticulatedTree *)tree)->SetShape(new Capsule(params));
00192       cnt++;
00193           }
00194           else{
00195             if(!type.compare("Sphere")){
00196               ((EmbodiedArticulatedTree *)tree)->SetShape(new Sphere(params));
00197               cnt++;
00198             }
00199             else{
00200               if(!type.compare("Parallelipiped")){
00201                 ((EmbodiedArticulatedTree *)tree)->
00202                   SetShape(new Parallelipiped(params));
00203               cnt++;
00204               }
00205             }
00206           }
00207         }
00208       }
00209     }
00210   }
00211   return cnt;
00212 }
00213 
00214 void  EmbodiedArticulatedTree::SetShape(Shape *sol){
00215   if(solid)delete solid;
00216   solid=sol;
00217 }
00218 
00219 void EmbodiedArticulatedTree::UpdateShape(){
00220     
00221     CVector3_t tr[10];//max number of children
00222     int i=0;
00223     ArticulatedTreeList_t::const_iterator it; 
00224     if(solid){    
00225         for(it=children.begin(); it!=children.end(); ++it){
00226             (*it)->GetJoint()->GetTranslation(tr[i]);
00227             i++;
00228         }
00229 
00230         solid->Update(tr,i);
00231     }
00232     for(it=children.begin(); it!=children.end(); ++it){
00233         ((EmbodiedArticulatedTree *)(*it))->UpdateShape();
00234     }  
00235 }
00236  
00237 
00238 EmbodiedKinematicTree::EmbodiedKinematicTree():KinematicTree(){
00239    e_root = (EmbodiedArticulatedTree *)root;
00240 }
00241 
00242 EmbodiedKinematicTree::EmbodiedKinematicTree(char *filename1, char *filename2){
00243   Tree *shape_tree = new Tree();
00244   xml_tree = new Tree();
00245   solid = NULL;
00246   xml_tree->LoadFromFile(string(filename1));
00247   root = new EmbodiedArticulatedTree(xml_tree);
00248   shape_tree->LoadFromFile(string(filename2));
00249   cout<<"file loaded"<<endl;
00250   e_root =(EmbodiedArticulatedTree *)root;
00251   e_root->SetAllShapes(shape_tree);
00252   cout<<(*e_root)<<endl;
00253 }
00254 
00255 
00256 int EmbodiedKinematicTree::CheckAllCollisions (KinematicTree *kt2=NULL){
00257 
00258   collision_arg_t arg;
00259   arg.dist = 0.1; 
00260   arg.kt_update = kt2;
00261   arg.kt = this;
00262   arg.kc = new EmbodiedKinematicChain();
00263   return root->CheckPairsOfNodes(EmbodiedKinematicTree::CollisionDetection,&arg);
00264 }
00265 
00278 int EmbodiedKinematicTree::CollisionDetection(pArticulatedTree_t t1, pArticulatedTree_t t2, void *arg){
00279  collision_arg_t *state;
00280  float distance=-1.0f;
00281  RigidTransfo rt;
00282  EmbodiedArticulatedTree *from,*to;
00283  state = (collision_arg_t *) arg;
00284  from = (EmbodiedArticulatedTree *)t1;
00285  to = (EmbodiedArticulatedTree *)t2;
00286  if(from->GetParent()!= to && to->GetParent()!= from){
00287    if(from->GetShape() && to->GetShape()){
00288      state->kt->LoadChain(t1,t2,state->kc, TOUCH_MODALITY);   
00289      state->kc->GlobalTransfo(rt);
00290      distance = from->GetShape()->Distance(*(to->GetShape()),rt);
00291 //      if(distance<0){
00292 //        cout<<from->name<<" - "<<to->name<<" "<<distance<<endl;
00293 //      }
00294      if(distance<=state->dist){
00295        char c=7;
00296        cout<<from->name<<" - "<<to->name<<" "<<distance<<endl;
00297        from->Highlight();
00298        to->Highlight();
00299        cout<<c;
00300        return 1;
00301      }
00302    }
00303  }
00304  return 0;
00305 }
 All Data Structures Functions Variables

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