Revision 1f748ce7

View differences:

examples/yarp_icub/include/icub_jointinterface.h
74 74 
    // yarp views
75 75 
    yarp::dev::IVelocityControl *yarp_ivel_;
76 76 
    yarp::dev::IPositionControl *yarp_ipos_;
77 

  		




  77
  78
  
    
    //yarp::dev::IEncodersTimed *yarp_iencs_;


  		




  78
  79
  
    
    yarp::dev::IControlLimits *yarp_ilimits_;


  		




  79
  80
  
    
    yarp::dev::IAmplifierControl *yarp_amp_;


  		




  ......




  105
  106
  
    
    void init_joints();


  		




  106
  107
  
    

  		




  107
  108
  
    
    std::string scope;


  		




  108
  
  
    
    yarp::sig::Vector positions;


  		




  109
  
  
    
    yarp::sig::Vector velocities;


  		




  
  109
  
    
    //yarp::sig::Vector positions;


  		




  
  110
  
    
    //yarp::sig::Vector velocities;


  		




  110
  111
  
    

  		




  111
  112
  
    

  		




  112
  113
  
    
    void store_min_max(yarp::dev::IControlLimits *ilimits, int id, int e);


  		




  113
  114
  
    

  		




  114
  
  
    
    float last_pos_eye_vergence;


  		




  115
  
  
    
    float last_pos_eye_pan;


  		




  116
  
  
    
    float last_vel_eye_vergence;


  		




  117
  
  
    
    float last_vel_eye_pan;


  		




  
  115
  
    
   //float last_pos_eye_vergence;


  		




  
  116
  
    
    //float last_pos_eye_pan;


  		




  
  117
  
    
   // float last_vel_eye_vergence;


  		




  
  118
  
    
    //float last_vel_eye_pan;


  		




  118
  119
  
    

  		




  119
  120
  
    
    void store_joint(int id, float value);


  		




  120
  121
  
    
    void set_target_in_positionmode(int id);


  		




  121
  122
  
    
    void set_target_in_velocitymode(int id);


  		




  122
  123
  
    

  		




  123
  124
  
    

  		




  124
  
  
    
    int convert_enum_to_motorid(int e);


  		




  125
  
  
    
    int convert_motorid_to_enum(int id);


  		




  
  125
  
    
    //int convert_enum_to_motorid(int e);


  		




  
  126
  
    
    //int convert_motorid_to_enum(int id);


  		




  126
  127
  
    

  		




  127
  128
  
    
    iCubFaceInterface *face_interface_;


  		




  128
  129
  
    

  		












  

    
      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>


  		




  5
  
  
    
//using namespace yarp::dev;


  		




  6
  
  
    
//using namespace yarp::sig;


  		




  7
  
  
    
//using namespace yarp::os;


  		




  
  4
  
    
//#include <boost/format.hpp>


  		




  8
  5
  
    
using std::cout;


  		




  
  6
  
    
using std::cerr;


  		




  9
  7
  
    
using std::string;


  		




  10
  
  
    

  		




  11
  8
  
    
using humotion::server::JointInterface;


  		




  12
  9
  
    
using yarp::dev::IEncodersTimed;


  		




  13
  10
  
    
using yarp::sig::Vector;


  		




  ......




  24
  21
  
    
    yarp::dev::PolyDriver *poly_driver = icub_jointinterface->get_yarp_polydriver();


  		




  25
  22
  
    
    bool success = poly_driver->view(iencs_);


  		




  26
  23
  
    
    if (!success) {


  		




  27
  
  
    
        cout << "ERROR: polydriver failed to init iencs view\n";


  		




  
  24
  
    
        cerr << "ERROR: polydriver failed to init iencs view\n";


  		




  28
  25
  
    
        exit(EXIT_FAILURE);


  		




  29
  26
  
    
    }


  		




  30
  27
  
    

  		




  ......




  43
  40
  
    
void iCubDataReceiver::threadRelease() {


  		




  44
  41
  
    
}


  		




  45
  42
  
    

  		




  46
  
  
    
yarp::sig::Vector iCubDataReceiver::calculate_velocities(yarp::sig::Vector positions,


  		




  47
  
  
    
                                                         yarp::sig::Vector timestamps) {


  		




  48
  
  
    
    yarp::sig::Vector velocities;


  		




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


  		




  
  44
  
    
    Vector velocities;


  		




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


  		




  50
  46
  
    

  		




  51
  
  
    
    cout << "\n";


  		




  52
  47
  
    
    if (previous_positions_.size() == 0){


  		




  53
  48
  
    
        // first run, no valid old position available, return zero velocities


  		




  54
  49
  
    
        // by setting all all elements to zero


  		




  ......




  60
  55
  
    
            float timediff = timestamps[i] - previous_timestamps_[i];


  		




  61
  56
  
    
            // calc speed:


  		




  62
  57
  
    
            velocities[i] = diff / timediff;


  		




  63
  
  
    
            cout << " [" << i << "]=" <<  boost::format("%6.3f") % velocities[i];


  		




  64
  58
  
    
        }


  		




  65
  59
  
    
    }


  		




  66
  
  
    
    cout << " VEL\n";


  		




  67
  60
  
    

  		




  68
  61
  
    
    previous_positions_ = positions;


  		




  69
  62
  
    
    previous_timestamps_ = timestamps;


  		












  

    
      examples/yarp_icub/src/icub_jointinterface.cpp
    
  





  298
  298
  
    
    //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);


  		




  299
  299
  
    

  		




  300
  300
  
    

  		




  301
  
  
    
    //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:


  		




  302
  
  
    
    face_interface_->set_eyelid_angle(target_angle_[ICUB_ID_EYES_RIGHT_LID_UPPER]);


  		




  
  301
  
    
    // eyelids: unfortuantely the icub has only 1dof for eyelids


  		




  
  302
  
    
    // therefore we can only use an opening value


  		




  
  303
  
    
    float opening_left  = target_angle_[ICUB_ID_EYES_LEFT_LID_UPPER]


  		




  
  304
  
    
                          - target_angle_[ICUB_ID_EYES_LEFT_LID_LOWER];


  		




  
  305
  
    
    float opening_right = target_angle_[ICUB_ID_EYES_RIGHT_LID_UPPER]


  		




  
  306
  
    
                          - target_angle_[ICUB_ID_EYES_RIGHT_LID_LOWER];


  		




  
  307
  
    
    float opening = (opening_left + opening_right) / 2.0;


  		




  
  308
  
    
    // send it to icub face if


  		




  
  309
  
    
    face_interface_->set_eyelid_angle(opening);


  		




  303
  310
  
    

  		




  304
  311
  
    
    //eyebrows are set using a special command as well:


  		




  305
  312
  
    
    face_interface_->set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW, target_angle_);


  		












Also available in:  Unified diff