BodySchemaLearning.h

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 
00216 #ifndef __BODY_SCHEMA_LEARNING_H__
00217 #define __BODY_SCHEMA_LEARNING_H__
00218 
00219 
00220 #include "KinematicTree.h"
00221 #include "BodySchema.h"
00222 //#include "KChainOrientationBodySchema.h"
00223 #include "yarp/os/all.h"
00224 #include "SpecializedPort.h"
00225 #include "yarp/os/Stamp.h"
00226 #include <iostream>
00227 #include <fstream>
00228 
00229 
00230 #define WITH_KIN_TREE
00231 
00232 
00233 class TimedBuffer;
00234 
00235 
00236 class BodySchemaLearningModule: public Module{
00237 
00238  protected:
00239 
00240   // communication ports
00241 
00242 
00243     JointAngleDataPort proprioception_head;
00244     JointAngleDataPort proprioception_arm;
00245     CartesianDataPort vision;
00246     VisualRotationPort visual_rotation_port;
00247     CommandPort commands;
00248 
00249     BodySchemaDataPort bsPort;
00250 
00251 #ifdef WITH_KIN_TREE
00252     KinematicTree *body;
00253     KinematicChain *eyes_arm;
00254     KinematicChain *eye_world;
00255     KinematicChain *head;
00256     KinematicChain *arm;
00257 #else
00258     KChainBodySchema body;
00259 #endif
00260 
00261     float outlier_thresh; 
00262   
00263       
00264     cart_vec_t markerPosition; 
00265     cart_vec_t visualPosition; 
00266     joint_vec_t arm_proprio;
00267     joint_vec_t head_proprio;
00269     float seenRotation[3];
00270     double t0,t1;
00271 
00272     int static_update;
00273     int rotation_update;
00274     
00275     
00276     int update_cnt; 
00277     int outputFrequency; 
00278     float *body_data;
00279     int body_data_size;  
00280 
00281     int arm_size;
00282 
00283     TimedBuffer *head_buffer;
00284     TimedBuffer *arm_buffer;
00285    
00286     // for logging 
00287     fstream log;
00288     bool logging;
00289     bool simul;
00290 
00291 protected:
00292     int loadChain(KinematicChain *chain,Searchable &s,const char *from, 
00293                   const char *to, const char *from_def, const char *to_def);
00294     int listen();
00295     bool updateBody();
00296     void bufferProprioception(){};//to implement
00297     bool listenToStaticVision();
00298     bool listenToVisualRotation();
00299   //  sendOutput();
00300 
00301  public:
00302     BodySchemaLearningModule();
00303     virtual ~BodySchemaLearningModule(){};
00304     virtual bool open(Searchable& config);
00305     virtual bool close();
00306     virtual bool updateModule();
00307     bool simulUpdate();
00308 };
00309 
00310 
00311 class TimedBuffer{
00312 protected:
00313     float *buffer;
00314     double *time_buf;
00315     int cnt;
00316     bool full;
00317     int width;
00318     int size;
00319 public:
00320     TimedBuffer(int size,int width);
00321     ~TimedBuffer();
00322     void add(float *values, double time);
00323     int lookFor(double time);
00324     float *get(int index);
00325     double getTime(int index){return time_buf[index];}
00326 };
00327 
00328 #endif 
 All Data Structures Functions Variables

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