Revision ad7b6f18 examples/yarp_icub/src/icub_data_receiver.cpp

View differences:

examples/yarp_icub/src/icub_data_receiver.cpp
1 1 
#include "icub_data_receiver.h"
2 2 
#include <humotion/server/joint_interface.h>
3 3 
#include <yarp/os/Property.h>
4 
//#include <boost/format.hpp>
4 
#include <boost/format.hpp>
5 

  		




  5
  6
  
    
using std::cout;


  		




  6
  7
  
    
using std::cerr;


  		




  7
  8
  
    
using std::string;


  		




  ......




  10
  11
  
    
using yarp::sig::Vector;


  		




  11
  12
  
    

  		




  12
  13
  
    
#define ICUB_DATA_RECEIVER_USE_ENCODERSPEED 0


  		




  
  14
  
    
#define ICUB_DATA_RECEIVER_DUMP_DATA 0


  		




  13
  15
  
    

  		




  14
  
  
    
iCubDataReceiver::iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface)


  		




  15
  
  
    
    : yarp::os::RateThread(period) {


  		




  
  16
  
    
//! constructor


  		




  
  17
  
    
//! \param period_ms for the yarp rate thread


  		




  
  18
  
    
//! \param icub_jointinterface


  		




  
  19
  
    
iCubDataReceiver::iCubDataReceiver(int period_ms, iCubJointInterface *icub_jointinterface)


  		




  
  20
  
    
    : yarp::os::RateThread(period_ms) {


  		




  16
  21
  
    

  		




  17
  22
  
    
    // store pointer to icub jointinterface


  		




  18
  23
  
    
    icub_jointinterface_ = icub_jointinterface;


  		




  ......




  33
  38
  
    
    timestamps_.resize(joints);


  		




  34
  39
  
    
}


  		




  35
  40
  
    

  		




  
  41
  
    
//! yarp rate thread initializer


  		




  36
  42
  
    
bool iCubDataReceiver::threadInit() {


  		




  37
  43
  
    
    return true;


  		




  38
  44
  
    
}


  		




  39
  45
  
    

  		




  
  46
  
    
//! yarp thread release function


  		




  40
  47
  
    
void iCubDataReceiver::threadRelease() {


  		




  41
  48
  
    
}


  		




  42
  49
  
    

  		




  
  50
  
    
//! manully calculate joint velocities (instead of using joint encoder speeds)


  		




  
  51
  
    
//! \param positions vector with current encoder position


  		




  
  52
  
    
//! \param timestamps vector with the associated timestamps


  		




  
  53
  
    
//! \return velocities as vector


  		




  43
  54
  
    
Vector iCubDataReceiver::calculate_velocities(Vector positions, Vector timestamps) {


  		




  44
  55
  
    
    Vector velocities;


  		




  45
  56
  
    
    velocities.resize(positions.size());


  		




  ......




  64
  75
  
    
    return velocities;


  		




  65
  76
  
    
}


  		




  66
  77
  
    

  		




  
  78
  
    
//! main loop routine, called by yarp rate thread


  		




  67
  79
  
    
void iCubDataReceiver::run() {


  		




  68
  80
  
    
    float velocity;


  		




  69
  81
  
    

  		




  ......




  89
  101
  
    
    //small hack to tell humotion to update the lid angle


  		




  90
  102
  
    
    //fixme: use real id


  		




  91
  103
  
    
    store_incoming_position(100, 0.0, timestamps_[0]);


  		




  
  104
  
    

  		




  
  105
  
    
    #if ICUB_DATA_RECEIVER_DUMP_DATA


  		




  
  106
  
    
    dump_incoming_data();


  		




  
  107
  
    
    #endif


  		




  92
  108
  
    
}


  		




  93
  109
  
    

  		




  94
  
  
    
void iCubDataReceiver::store_incoming_position(int icub_id, double value, double timestamp) {


  		




  95
  
  
    
    cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << value << ")\n";


  		




  
  110
  
    
//! store incoming position for a given icub joint


  		




  
  111
  
    
//! \param icub _id icub joint id


  		




  
  112
  
    
//! \param position


  		




  
  113
  
    
//! \param timestamp


  		




  
  114
  
    
void iCubDataReceiver::store_incoming_position(int icub_id, double position, double timestamp) {


  		




  
  115
  
    
    cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << position << ")\n";


  		




  96
  116
  
    

  		




  97
  117
  
    
    // store joint position in humotion backend


  		




  98
  118
  
    
    if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||


  		




  ......




  101
  121
  
    
        // instead of using seperate left/right pan the icub uses


  		




  102
  122
  
    
        // a combined pan angle and vergence. therfore we have to convert this here:


  		




  103
  123
  
    
        if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {


  		




  104
  
  
    
            target_eye_pan_ = value;


  		




  
  124
  
    
            target_eye_pan_ = position;


  		




  105
  125
  
    
        } else {


  		




  106
  
  
    
            target_eye_vergence_ = -value;


  		




  
  126
  
    
            target_eye_vergence_ = -position;


  		




  107
  127
  
    
        }


  		




  108
  128
  
    

  		




  109
  129
  
    
        float left  = target_eye_pan_ + target_eye_vergence_/2.0;


  		




  ......




  120
  140
  
    
    } else {


  		




  121
  141
  
    
        if (icub_id == iCubJointInterface::ID_NECK_PAN) {


  		




  122
  142
  
    
            // icub uses an inverted neck pan specification


  		




  123
  
  
    
            value = -value;


  		




  
  143
  
    
            position = -position;


  		




  124
  144
  
    
        }


  		




  125
  145
  
    

  		




  126
  146
  
    
        // known configured mapping between joint ids


  		




  127
  147
  
    
        int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);


  		




  128
  
  
    
        icub_jointinterface_->store_incoming_position(humotion_id, value, timestamp);


  		




  
  148
  
    
        icub_jointinterface_->store_incoming_position(humotion_id, position, timestamp);


  		




  129
  149
  
    
    }


  		




  130
  150
  
    
}


  		




  131
  151
  
    

  		




  
  152
  
    
//! store incoming velocity for a given icub joint


  		




  
  153
  
    
//! \param icub_id icub joint id


  		




  
  154
  
    
//! \param velocity


  		




  
  155
  
    
//! \param timestamp


  		




  132
  156
  
    
void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, double timestamp) {


  		




  133
  157
  
    
    cout << "iCubDataReceiver::store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";


  		




  134
  158
  
    

  		




  ......




  167
  191
  
    
    }


  		




  168
  192
  
    
}


  		




  169
  193
  
    

  		




  
  194
  
    
//! helper for debugging purposes, feed this data into gnuplot for visual inspection


  		




  
  195
  
    
void iCubDataReceiver::dump_incoming_data() {


  		




  
  196
  
    
    // use gnuplot for viz:


  		




  
  197
  
    
    // ./icub_humotion_server --robot icub | grep "INCOMING" |tee  log


  		




  
  198
  
    
    // @gnuplot: plot "log" using 0:1 w l t "p neck tilt", "log" using 0:2 w l t "v neck tilt", \


  		




  
  199
  
    
    //                "log" using 0:5 w l t "p neck pan", "log" using 0:6 w l t "v neck pan", \


  		




  
  200
  
    
    //                "log" using 0:7 w l t "p eyes ud", "log" using 0:8 w l t "v eyes ud", \


  		




  
  201
  
    
    //                "log" using 0:9 w l t "p eyes vergence", "log" using 0:10 w l t "v eyes vergence"


  		




  
  202
  
    
    cout << "\n";


  		




  
  203
  
    
            // publish data to humotion


  		




  
  204
  
    
    for(int i=0; i<positions_.size(); i++){


  		




  
  205
  
    
        cout << positions_[i] << " ";


  		




  
  206
  
    
        cout << velocities_[i] << " ";


  		




  
  207
  
    
    }


  		




  
  208
  
    
    cout << " #INCOMING_DATA_DUMP\n";


  		




  
  209
  
    
}


  		




  
  210
  
    

  		




  170
  211
  
    

  		












Also available in:  Unified diff