PerceptionSimulationModule.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C)
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * author: Micha Hersch
00006  * email:   micha.hersch@robotcub.org
00007  * website: www.robotcub.org
00008  * Permission is granted to copy, distribute, and/or modify this program
00009  * under the terms of the GNU General Public License, version 2 or any
00010  * later version published by the Free Software Foundation.
00011  *
00012  * A copy of the license can be found at
00013  * http://www.robotcub.org/icub/license/gpl.txt
00014  *
00015  * This program is distributed in the hope that it will be useful, but
00016  * WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00018  * Public License for more details
00019  */
00020 #include <ace/OS_NS_stdio.h>
00021 #include "PerceptionSimulationModule.h"
00022 
00023 
00024 int cartesian_dim=0;
00025 int joint_angle_dim =0;
00026 
00027 PerceptionSimulationModule::PerceptionSimulationModule(){
00028   delay=0.2;
00029   last_time =0;
00030 }
00031 PerceptionSimulationModule::~PerceptionSimulationModule(){}
00032 
00033 int PerceptionSimulationModule::loadChain(KinematicChain *chain,Searchable &s,
00034                                            const char *from, const char *to, 
00035                                            const char *from_def, const char *to_def){
00036 
00037     Value& vfrom =s.find(from);
00038     Value& vto = s.find(to);
00039     if(! vfrom.isNull() && ! vto.isNull()){
00040         if(!body->LoadChain(vfrom.asString().c_str(),vto.asString().c_str(),chain)){
00041             cout<<"cannot load chain going from "<<vfrom.asString().c_str()<<" to "
00042                 <<vto.asString().c_str()<<endl;
00043         }
00044     }
00045     else{// default values
00046         body->LoadChain(from_def,to_def,chain);
00047     }
00048     cout<<"kinematic chain found ("<<chain->GetNbJoints()<<" joints)"<<endl;
00049     return chain->GetNbJoints();
00050 }
00051 
00052 
00053 bool PerceptionSimulationModule::open(Searchable &s){
00054  Value& struc = s.find("structure");
00055  if(struc.isNull()){
00056    ACE_OS::printf("no robot structure file given\n");
00057    return false;
00058  }
00059  cartesian_dim = 3; //to do before call to "new body"
00060  body = new KinematicTree(struc.asString().c_str());
00061  eyes_arm = body->GetChain(0);
00062  eye_world = body->GetChain(1);
00063  head = body->GetChain(2); 
00064  arm = body->GetChain(3); 
00065 
00066  if(!loadChain(head,s,"head_base","head_end","neck_tilt","r_eye")){
00067      head=NULL;
00068  }
00069  if(!loadChain(arm,s,"arm_base","arm_end","r_sfe","r_hand")){
00070      arm=NULL;
00071  }
00072  if(!loadChain(eye_world,s,"eye","world","neck_tilt","r_eye")){
00073      eye_world=NULL;
00074  }
00075 
00076  if(s.check("no_static")){
00077      eyes_arm=NULL;
00078  }
00079  else{
00080      if(!loadChain(eyes_arm,s,"stereo","marker","eyes","r_hand")){
00081          eyes_arm=NULL;
00082      }
00083  }
00084 
00085  if(arm){arm_angles.Resize(arm->GetNbJoints());}
00086  if(head){ head_angles.Resize(head->GetNbJoints());}
00087  position.Resize(cartesian_dim);
00088 
00089  // opening communication ports 
00090 
00091  if(!proprioception_head.open(getName("proprioception_head:o").c_str())){
00092    ACE_OS::printf("Cannot open port %s\n",getName("proprioception_head:o").c_str());
00093    return false;
00094  }
00095  if(!proprioception_arm.open(getName("proprioception_arm:o").c_str())){
00096    ACE_OS::printf("Cannot open port %s\n",getName("proprioception_arm:o").c_str());
00097    return false;
00098  }
00099  
00100  if(!efferent_head.open(getName("efferent_head:i").c_str())){
00101      ACE_OS::printf("Cannot open port %s\n",getName("efferent_head:i").c_str());
00102      return false;
00103  }
00104  efferent_head.setStrict(false);
00105 
00106  if(!efferent_arm.open(getName("efferent_arm:i").c_str())){
00107      ACE_OS::printf("Cannot open port %s\n",getName("efferent_arm:i").c_str());
00108      return false;
00109  }
00110  efferent_arm.setStrict(false);
00111 
00112 
00113  if(!vision.open(getName("vision:o").c_str())){
00114    ACE_OS::printf("Cannot open port %s\n",getName("vision:o").c_str());
00115    return false;
00116   }
00117  if(!visualRotation.open(getName("visual_rotation:o").c_str())){
00118    ACE_OS::printf("Cannot open port %s\n",getName("visual_rotation:o").c_str());
00119    return false;
00120   }
00121 
00122  if(!cmdPort.open(getName("cmd:i").c_str())){
00123    ACE_OS::printf("Cannot open port %s\n",getName("cmd:i").c_str());
00124    return false;
00125   }
00126 
00127 
00128 //  vision.addOutput("/vision/in");
00129 //  proprioception.addOutput("proprioception/in");
00130 
00131  Value& opt = s.find("delay");
00132  if(!opt.isNull()){
00133      delay = opt.asDouble();
00134  }
00135 
00136  
00137  return true;
00138 }
00139 
00140 
00141 
00142 
00143 
00144 bool PerceptionSimulationModule::updateModule(){
00145     
00146     Rotation nrot;
00147     int new_pos = 0;
00148  
00149     Time::delay(delay);
00150     if(cmdPort.ShouldQuit()){return false;}
00151     if(efferent_head.getInputCount()>0 && (efferent_arm.getInputCount()>0 || ! eyes_arm)){
00152         if(efferent_head.ReadPosition(head_angles.GetArray(),head_angles.Size())){
00153             head_angles *= PIf/180.0;
00154             head->SetAngles(head_angles.GetArray());
00155             new_pos =1;
00156         }
00157         if(efferent_arm.ReadPosition(arm_angles.GetArray(),arm_angles.Size())){
00158             arm_angles *= PIf/180.0;
00159             arm->SetAngles(arm_angles.GetArray());
00160         }       
00161     }
00162     else{
00163         body->GetArticulatedTree()->RandomAngle();
00164         head->GetAngles(head_angles.GetArray());
00165         arm->GetAngles(arm_angles.GetArray());
00166     }
00167     
00168     stamp.update();   
00169     //    cout<<"stamp "<<stamp.getTime()<<endl;
00170     if(new_pos){
00171         if(eye_world){
00172             eye_world->GlobalRotation(nrot);
00173             rot.InvertRotationAngle();
00174             rot = rot*nrot;
00175             visualRotation.SendRotation(rot.GetRotationParam(),last_time,stamp.getTime());
00176             rot = nrot;
00177         }
00178         
00179      if(eyes_arm){
00180          eyes_arm->ForwardKinematics(position.GetArray());
00181          //       cout<<"sending ";position.Print();
00182          vision.setEnvelope(stamp);
00183          vision.SendPosition(position.GetArray());    
00184      }
00185  
00186      head_angles *= 180.0/PIf;
00187      arm_angles *= 180.0/PIf;
00188 
00189      proprioception_head.setEnvelope(stamp);
00190      proprioception_head.SendPosition(head_angles.GetArray(),head_angles.Size());
00191      proprioception_arm.setEnvelope(stamp);   
00192      proprioception_arm.SendPosition(arm_angles.GetArray(),arm_angles.Size());
00193      last_time = stamp.getTime();
00194  //     cout<<"time "<<stamp.getTime()<<endl;
00195 //      head_angles.Print();
00196 //      arm_angles.Print();
00197 //      position.Print();
00198          }
00199     
00200       return true;
00201 }
00202 
00203 bool PerceptionSimulationModule::close(){
00204   vision.close();
00205   visualRotation.close();
00206   proprioception_arm.close();
00207   proprioception_head.close();
00208   efferent_head.close();
00209   efferent_arm.close();
00210   cmdPort.close();
00211   delete body;
00212   return true;
00213 }
00214 
00215 // bool PerceptionSimulationModule::respond(const Bottle& command, Bottle& reply) {
00216 //   switch (command.get(0).asVocab()){
00217 //   case VOCAB1('+'):{
00218 //     delay += 0.1;
00219 //     // delay = max(0,delay);
00220 //     reply.addVocab(Vocab::encode("delay"));
00221 //     reply.addDouble(delay);
00222 //     return true;
00223 //   }
00224 //   case VOCAB1('-'):{
00225 //     delay -= 0.1;
00226 //     // delay = max(0,delay);
00227 //     reply.addVocab(Vocab::encode("delay"));
00228 //     reply.addDouble(delay);
00229 //     return true;
00230 //   }
00231 //   default:
00232 //     reply.add("command not recognized");
00233 //     return false;
00234 //   }
00235 //   return false;
00236 // }
00237 
00238 int usage(){ 
00239   cout<<"usage: perception_module [--file <config_file> | --structure <structure_file>] [--delay <delay[s]>]"<<endl;
00240   return 1;
00241 }
00242 
00243 int main(int argc, char *argv[]){
00244   PerceptionSimulationModule module; 
00245   Property prop;
00246   if(argc==1){
00247     return usage();
00248   }
00249   if(prop.check("help")){
00250     return usage();
00251   } 
00252   module.runModule(argc,argv);
00253 }
 All Data Structures Functions Variables

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