BodySchemaLearning.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 
00021 #define ICUB_IS_HERE
00022 
00023 #include <ace/OS_NS_stdio.h>
00024 #include "BodySchemaLearning.h"
00025 
00026 
00027 int cartesian_dim=3;
00028 int joint_angle_dim=0;
00029 
00030 
00031 
00032 
00033 BodySchemaLearningModule::BodySchemaLearningModule(){
00034   update_cnt =0;
00035   body_data =NULL;
00036   outputFrequency = 50;
00037   outlier_thresh = 500;
00038   static_update= 0;
00039   rotation_update= 0;
00040 }
00041 
00042 int BodySchemaLearningModule::loadChain(KinematicChain *chain,Searchable &s,
00043                                            const char *from, const char *to, 
00044                                            const char *from_def, const char *to_def){
00045 
00046     Value& vfrom =s.find(from);
00047     Value& vto = s.find(to);
00048     if(! vfrom.isNull() && ! vto.isNull()){
00049         if(!body->LoadChain(vfrom.asString().c_str(),vto.asString().c_str(),chain)){
00050             cout<<"cannot load chain going from "<<vfrom.asString().c_str()<<" to "
00051                 <<vto.asString().c_str()<<endl;
00052         }
00053     }
00054     else{// default values
00055         body->LoadChain(from_def,to_def,chain);
00056     }
00057     cout<<"kinematic chain found ("<<chain->GetNbJoints()<<" joints)"<<endl;
00058     return chain->GetNbJoints();
00059 }
00060 
00061 bool BodySchemaLearningModule::open(Searchable& s){
00062   //  char tmp1[100],tmp2[100];
00063 
00064   //Loading the body schema structure
00065   Value& struc = s.find("structure");
00066   if(struc.isNull()){
00067     ACE_OS::printf("no robot structure file given\n");
00068     return false;
00069   }
00070 
00071 
00072   body = new KinematicTree(struc.asString().c_str());
00073   body_data_size = body->GetTreeSize()*6;
00074   body_data = new float[body_data_size];
00075 
00076   eyes_arm  = body->GetChain(0);
00077   eye_world = body->GetChain(1);
00078   head = body->GetChain(2); 
00079   arm = body->GetChain(3);
00080 
00081 #define FOR_ERIC //who does not want to learn the head body schema
00082 #ifdef FOR_ERIC  // quick and dirty before the demo
00083   string jnames[4] = {"neck_tilt","neck_swing","neck_pan","eye_tilt"};
00084   for(int i=0;i<4;i++){
00085       body->GetArticulatedTree()->FindJoint(jnames[i])->GetJoint()->NoLearning();
00086   }
00087 #endif
00088 
00089 
00090 
00091   if(!loadChain(head,s,"head_base","head_end","neck_tilt","r_eye")){
00092       head=NULL;
00093   }
00094   if(!loadChain(arm,s,"arm_base","arm_end","r_sfe","r_hand")){
00095       arm=NULL;
00096   }
00097  if(!loadChain(eye_world,s,"eye","world","neck_tilt","r_eye")){
00098      eye_world=NULL;
00099  }
00100 
00101  if(s.check("no_static")){
00102      eyes_arm=NULL;
00103  }
00104  else{
00105      if(!loadChain(eyes_arm,s,"stereo","marker","eyes","r_hand")){
00106          eyes_arm=NULL;
00107      }
00108  }
00109  
00110  if(arm){arm_proprio.Resize(arm->GetNbJoints());}
00111  if(head){ head_proprio.Resize(head->GetNbJoints());}
00112 
00113 
00114   Value& buffer = s.find("bufferSize");
00115   int buff_size = buffer.isNull()?30:buffer.asInt();//30: default value
00116   head_buffer = new TimedBuffer(buff_size, head->GetNbJoints());
00117   arm_buffer = new TimedBuffer(buff_size,arm->GetNbJoints());
00118 
00119 
00120 
00121   if(s.check("random_head")){
00122       //      head->RandomAxes();
00123       head->SetRotationAxis(0,0.f,1.f,0.f);
00124       head->SetRotationAxis(2,1.f,0.f,0.f);
00125       head->SetRotationAxis(3,0.f,0.f,1.f);
00126   }
00127 
00128 
00129   Value& logfile = s.find("logfile");
00130   if(!logfile.isNull()){
00131       log.open(logfile.asString().c_str());
00132       logging = true;
00133       if(log.fail()){
00134           cout<<"cannot open file "<<logfile.asString().c_str()<<endl;
00135           return false;
00136       }
00137       if(s.check("catchup")){
00138           cout<<"catching up from "<<logfile.asString().c_str()<<endl;
00139           int ccnt=0;
00140           while(simulUpdate()){ccnt++;}
00141           log.seekp(0,ios_base::end);
00142           cout<<"loaded "<<ccnt<<" points"<<endl;
00143           
00144       }
00145   }
00146   else{
00147       logging = false;
00148   }
00149 
00150 
00151   if(s.check("simulation")){ //no port in that case     
00152       simul = true;
00153       return true;
00154   }
00155   else{
00156       simul = false;
00157   }
00158 
00159   Value& opt = s.find("frequency");
00160   if(! opt.isNull()){
00161     outputFrequency = opt.asInt();
00162   }
00163 
00164   opt = s.find("rate");
00165   if(! opt.isNull()){
00166     //   body.SetRate(opt.asDouble()); later
00167   }
00168   
00169   Value& thresh = s.find("threshold");
00170   if(!thresh.isNull()){
00171      outlier_thresh = thresh.asDouble();
00172      cout<<"threshold "<<outlier_thresh<<endl;
00173   }
00174   markerPosition.Zero();
00175 
00176   //Opening the communication ports
00177   if(!commands.open(getName("cmd:i").c_str())){
00178     ACE_OS::printf("Cannot open port %s\n",getName("cmd:i").c_str());
00179      return false;
00180   }
00181     
00182   if(!bsPort.open(getName("body_schema:o").c_str())){
00183     ACE_OS::printf("Cannot open port %s\n",getName("body_schema:o").c_str());
00184     return false;
00185   }
00186 
00187   if(!proprioception_arm.open(getName("proprioception_arm:i").c_str())){
00188     ACE_OS::printf("Cannot open port %s\n",getName("proprioception_arm:i").c_str());
00189     return false;
00190   }
00191 
00192 
00193   if(!proprioception_head.open(getName("proprioception_head:i").c_str())){
00194     ACE_OS::printf("Cannot open port %s\n",getName("proprioception_head:i").c_str());
00195     return false;
00196   }
00197 
00198   if(!vision.open(getName("vision:i").c_str())){
00199     ACE_OS::printf("Cannot open port %s\n",getName("vision:i").c_str());
00200     return false;
00201   }
00202 
00203  if(!visual_rotation_port.open(getName("visual_rotation:i").c_str())){
00204     ACE_OS::printf("Cannot open port %s\n",getName("visual_rotation:i").c_str());
00205     return false;
00206   }
00207 
00208      //sending out initial body schema
00209  Time::delay(5);
00210  body->GetArticulatedTree()->Serialize(body_data,body_data_size);
00211  bsPort.SendBodySchema(body_data,body_data_size);
00212  
00213  return true;
00214 }
00215 
00216 
00217 int BodySchemaLearningModule::listen(){
00218       Stamp stamp;
00219       if(proprioception_head.ReadPosition(head_proprio.GetArray(),head_proprio.Size())){
00220         if(proprioception_head.getEnvelope(stamp)){
00221             head_proprio *= PIf/180;
00222             head_buffer->add(head_proprio.GetArray(),stamp.getTime());
00223         }
00224         else{
00225             cout<<"cannot read head stamp"<<endl;
00226         }
00227     }
00228     
00229     if(proprioception_arm.ReadPosition(arm_proprio.GetArray(),arm_proprio.Size())){
00230         if(proprioception_arm.getEnvelope(stamp)){
00231             arm_proprio *= PIf/180;
00232             arm_buffer->add(arm_proprio.GetArray(),stamp.getTime());
00233         }
00234         else{
00235             cout<<"cannot read arm stamp"<<endl;
00236         }
00237     }
00238     return 1;
00239 }
00240 
00241 bool BodySchemaLearningModule::listenToStaticVision(){
00242     if(vision.ReadPosition(visualPosition.GetArray())){
00243         static_update=1;
00244         // get the time stamp
00245         //possibly do some conversion
00246         return true;
00247     }
00248     return false;
00249 }
00250 
00251 
00252 bool BodySchemaLearningModule::listenToVisualRotation(){
00253     if(visual_rotation_port.ReadRotation(seenRotation,&t0,&t1)){
00254         rotation_update=1;
00255         return true;
00256     }
00257     return false;
00258 }
00259 
00260 // #ifdef ICUB_IS_HERE
00261 //        // float tmp= visualPosition.GetArray()[0];
00262 // //        visualPosition.GetArray()[0]=-visualPosition.GetArray()[1];
00263 // //        visualPosition.GetArray()[1]=-tmp;
00264 // //        visualPosition.GetArray()[2]*=-1; 
00265 //        float tmp= visualPosition.GetArray()[0];
00266 //        visualPosition.GetArray()[0]=-visualPosition.GetArray()[1]+95;
00267 //        visualPosition.GetArray()[1]=tmp-178;
00268 //        visualPosition.GetArray()[2]+=451;
00269 // #endif
00270 //     }
00271 
00272 
00273 // void BodySchemaLearningModule::sendOutput(){}
00274 
00275 
00276 bool BodySchemaLearningModule::close(){
00277   commands.close();//update to stop
00278   bsPort.close();//update to stop
00279   proprioception_arm.close();
00280   vision.close();
00281   proprioception_head.close();
00282   visual_rotation_port.close();
00283   if(logging){log.close();}
00284   if(body_data){ delete[] body_data;}
00285   delete head_buffer;
00286   delete arm_buffer;
00287   delete body;
00288   return true;
00289 }
00290 
00291 
00292 
00293 bool BodySchemaLearningModule::updateBody(){
00294     float dist, drot;
00295     int ind_h, ind_a;
00296     dist=drot=-1.f;
00297 //   visualPosition.Print();
00298 //   proprioceptivePosition.Print();
00299     if(eyes_arm){
00300         if(vision.ReadPosition(visualPosition.GetArray())){
00301             //only for this calibration
00302             visualPosition[0] *= -1;
00303             visualPosition[1] *= -1;
00304             Stamp stamp;
00305             if(vision.getEnvelope(stamp)){
00306                 ind_h = head_buffer->lookFor(stamp.getTime());
00307                 if(ind_h >=0){
00308                     ind_a = arm_buffer->lookFor(stamp.getTime());
00309                     if(ind_a >=0){
00310                         Vector angles(eyes_arm->GetNbJoints());
00311                         head->SetAngles(head_buffer->get(ind_h));
00312                         arm->SetAngles(arm_buffer->get(ind_a));
00313                         eyes_arm->GetAngles(angles.GetArray());//update could be optimized to avoid setting angles twice
00314                         log<<"1 "<<angles<<visualPosition<<outlier_thresh<<endl;
00315                         dist = eyes_arm->Update(angles.GetArray(),visualPosition.GetArray(),outlier_thresh); //could add marker position
00316                         for(int i=0;i<3;i++){
00317                             eyes_arm->Update(angles.GetArray(),visualPosition.GetArray(),outlier_thresh); //could add marker position             
00318                         }               
00319                     }
00320                     else{
00321                         cout<<"cannot find stamp in arm buffer "<<stamp.getTime()<<endl;
00322                     }        
00323                 }
00324                 else{
00325                 cout<<"cannot find stamp in head buffer "<<stamp.getTime()<<endl;
00326                 }
00327             }
00328             else{
00329                 cout<<"cannot read envelope on vision buffer "<<stamp.getTime()<<endl;
00330             }
00331         }
00332     }
00333     if(visual_rotation_port.ReadRotation(seenRotation,&t0,&t1)){
00334       Vector ew0_angles(eye_world->GetNbJoints());
00335       Vector ew1_angles(eye_world->GetNbJoints());
00336       Vector ew_diff(eye_world->GetNbJoints());
00337         
00338       // find the position at time t1
00339       int ind = head_buffer->lookFor(t1);
00340       if(ind>=0){
00341           head->SetAngles(head_buffer->get(ind));
00342           eye_world->GetAngles(ew1_angles.GetArray());
00343           
00344           
00345           //finding the position at time t0
00346           ind = head_buffer->lookFor(t0);
00347           if(ind>=0){
00348               head->SetAngles(head_buffer->get(ind));
00349               eye_world->GetAngles(ew0_angles.GetArray());
00350               
00351               ew_diff = ew1_angles - ew0_angles;
00352               //             ew0_angles.Print();
00353               //               ew_diff.Print();
00354               log<<"0 "<<ew0_angles<<ew_diff<<seenRotation[0]<<" "<<seenRotation[1]<<
00355                  " "<<seenRotation[2]<<endl;
00356               for(int i=0;i<3;i++){
00357                   drot = eye_world->UpdateWithFullJacobian(ew0_angles.GetArray(),ew_diff.GetArray(),seenRotation);
00358               }
00359               rotation_update = 0;
00360           }
00361           else{
00362               cout<<"cannot find time in head buffer "<<t0<<endl;;
00363           }
00364       }
00365       else{
00366           cout<<"cannot find time in head buffer "<<t1<<endl;;
00367       }
00368   }
00369     bool ret_val = dist > 0 || drot>0;
00370     if(ret_val){
00371         if(!(rand()%1))cout<<"drot "<<drot<<" dist "<<dist<<endl;
00372     }
00373     return ret_val;
00374 }
00375 
00376 bool BodySchemaLearningModule::updateModule(){
00377     if(simul)return simulUpdate();
00378     if(commands.ShouldQuit()){return false;}
00379     listen();
00380     if(updateBody()){
00381         if((++update_cnt) == outputFrequency){
00382             body->GetArticulatedTree()->Serialize(body_data,body_data_size);
00383             bsPort.SendBodySchema(body_data,body_data_size);   
00384             update_cnt=0;
00385         }
00386     }
00387     //  else{
00388     //   Time::delay(0.1);
00389     // }
00390     return true;
00391 }
00392 
00393 
00394 bool BodySchemaLearningModule::simulUpdate(){
00395     Vector ew_angles(eye_world->GetNbJoints());
00396     Vector ew_diff(eye_world->GetNbJoints());
00397     Vector angles(eyes_arm->GetNbJoints());
00398 
00399     //  cout<<"char "<<c<<endl;
00400     float dist,drot;
00401 
00402   //   log.seekg (0, ios::end);
00403 //     int length = log.tellg();
00404 //     log.seekg ((int)RND(length), ios::beg);
00405 //     log.ignore(300,'\n');
00406 
00407     char c =log.peek();
00408     if (log.eof()){
00409         cout<<"end of file"<<endl;
00410         log.seekg(0);
00411         log.clear();
00412         return (++update_cnt) < 1;
00413     }
00414     if(c=='1'){
00415         log>>c;
00416         for(unsigned int i=0;i<angles.Size();i++){
00417             log>>angles.GetArray()[i];
00418         }
00419         for(unsigned int i=0;i<visualPosition.Size();i++){
00420             log>>visualPosition.GetArray()[i];
00421         }
00422         log>>outlier_thresh;
00423  //        angles.Print();
00424 //         visualPosition.Print();
00425         //only for this calibration
00426 //         visualPosition[0] *= -1;
00427 //         visualPosition[2] *= -1;
00428         dist = eyes_arm->Update(angles.GetArray(),visualPosition.GetArray(),outlier_thresh);
00429         cout<<"dist "<<dist<<endl;
00430         cout<<"h_axes ";head->PrintAxes(cout);
00431         cout<<"a_axes ";arm->PrintAxes(cout);
00432         for(int i=0;i<3;i++)
00433             dist = eyes_arm->Update(angles.GetArray(),visualPosition.GetArray(),outlier_thresh);
00434     }
00435     else{
00436         log>>c;
00437         for(unsigned int i=0;i<ew_angles.Size();i++){
00438             log>>ew_angles.GetArray()[i];
00439         }
00440         for(unsigned int i=0;i<ew_diff.Size();i++){
00441             log>>ew_diff.GetArray()[i];
00442         }
00443         //        ew_angles.Print();
00444         log>>seenRotation[0]>>seenRotation[1]>>seenRotation[1];
00445         
00446         //uncomment for data analysis
00447         for(int i=0;i<4;i++){
00448             drot = eye_world->UpdateWithFullJacobian(ew_angles.GetArray(),
00449                                                      ew_diff.GetArray(),seenRotation);
00450         }
00451         cout<<"h_axes ";head->PrintAxes(cout);
00452 
00453     }
00454     log.ignore(2,'\n');
00455 //     update_cnt++;  
00456 //     if (update_cnt==2)return false;
00457     return true;
00458 }
00459 
00460 
00461 
00462 
00463 int usage(){
00464     cout<<"usage: body_schema_module [--file <config_file> | --structure <structure_file>] "
00465         <<"[--name <port_base_name>] [--threshold <outlier_threshold>] "
00466         <<"[--frequency <output_frequenct>] [--rate <learning_rate>] ]"<<endl;//update
00467     return 1;
00468 }
00469 
00470 
00471 TimedBuffer::TimedBuffer(int si, int wi){
00472     size= si;
00473     width = wi;
00474     buffer = new float[size*width];
00475     time_buf = new double[size];
00476     cnt=0;
00477     full=false;
00478 }
00479 
00480 TimedBuffer::~TimedBuffer(){
00481     delete[] buffer;
00482     delete[] time_buf;
00483 }
00484 
00485 void TimedBuffer::add(float *v, double t){
00486     memcpy(buffer+cnt*width,v,width*sizeof(float));
00487     time_buf[cnt] = t;
00488     cnt++;
00489     if(cnt==size){
00490         cnt=0;
00491         full=true;
00492     }
00493 }
00494 // assumed time entries are more or less increasing
00495 
00496 float *TimedBuffer::get(int index){
00497     return buffer+index*width;
00498 }
00499 
00500 int TimedBuffer::lookFor(double t){
00501     int i=cnt;
00502     int lasti=cnt;
00503     while(true){
00504         if(i<0){
00505             if(full){ //wrapping 
00506                 i= size-1;
00507             }
00508             else{
00509                 return -1;
00510             }
00511         }
00512         // buffer entirely visited
00513         if(i==cnt && lasti==cnt+1){
00514             cout<<"index not found";
00515             return -1;
00516         }
00517         //found
00518         if(time_buf[lasti]>=t && time_buf[i]<=t){
00519             return t>0.5*(time_buf[i]+time_buf[lasti])?lasti:i;
00520         }
00521         lasti=i;
00522         i--;
00523     }
00524 }
00525 
00526 
00527 int main(int argc, char *argv[]){
00528   BodySchemaLearningModule module;
00529   Property prop;
00530   srand(time(NULL));
00531   if(argc==1){
00532     return usage();
00533   }
00534   if(prop.check("help")){
00535     return usage();
00536   } 
00537   module.runModule(argc,argv);
00538 }
00539 
 All Data Structures Functions Variables

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