Revision a5cc61d4

View differences:

examples/humotion_yarp_icub/include/humotion_yarp_icub/icub_data_receiver.h
25 25 
* Excellence Initiative.
26 26 
*/
27 27 

  		




  28
  
  
    
#ifndef EXAMPLES_YARP_ICUB_INCLUDE_ICUB_DATA_RECEIVER_H_


  		




  29
  
  
    
#define EXAMPLES_YARP_ICUB_INCLUDE_ICUB_DATA_RECEIVER_H_


  		




  
  28
  
    
#ifndef EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_DATA_RECEIVER_H_


  		




  
  29
  
    
#define EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_DATA_RECEIVER_H_


  		




  30
  30
  
    

  		




  31
  31
  
    
#include <humotion/server/joint_interface.h>


  		




  32
  32
  
    
#include <humotion/server/server.h>


  		




  ......




  41
  41
  
    
#include <yarp/os/Time.h>


  		




  42
  42
  
    
#include <yarp/sig/Vector.h>


  		




  43
  43
  
    

  		




  44
  
  
    
#include "icub_humotion/icub_jointinterface.h"


  		




  
  44
  
    
#include "humotion_yarp_icub/icub_jointinterface.h"


  		




  45
  45
  
    

  		




  46
  46
  
    
// forward declaration to solve loop


  		




  47
  47
  
    
class iCubJointInterface;


  		




  48
  48
  
    

  		




  49
  49
  
    
class iCubDataReceiver : public yarp::os::RateThread{


  		




  50
  
  
    
public:


  		




  
  50
  
    
 public:


  		




  51
  51
  
    
    iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface);


  		




  52
  52
  
    
    bool threadInit();


  		




  53
  53
  
    
    void threadRelease();


  		




  54
  54
  
    
    void run();


  		




  55
  
  
    
private:


  		




  
  55
  
    

  		




  
  56
  
    
 private:


  		




  56
  57
  
    
    void store_incoming_position(int icub_id, double value, double timestamp);


  		




  57
  58
  
    
    void store_incoming_velocity(int icub_id, double velocity, double timestamp);


  		




  58
  59
  
    
    yarp::sig::Vector calculate_velocities(yarp::sig::Vector positions,


  		




  ......




  78
  79
  
    
    yarp::dev::IEncodersTimed *iencs_;


  		




  79
  80
  
    
};


  		




  80
  81
  
    

  		




  81
  
  
    
#endif  // EXAMPLES_YARP_ICUB_INCLUDE_ICUB_DATA_RECEIVER_H_


  		




  
  82
  
    
#endif  // EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_DATA_RECEIVER_H_


  		












  

    
      examples/humotion_yarp_icub/include/humotion_yarp_icub/icub_faceinterface.h
    
  





  24
  24
  
    
* Forschungsgemeinschaft (DFG) in the context of the German


  		




  25
  25
  
    
* Excellence Initiative.


  		




  26
  26
  
    
*/


  		




  27
  
  
    
#pragma once


  		




  
  27
  
    
#ifndef EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_FACEINTERFACE_H_


  		




  
  28
  
    
#define EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_FACEINTERFACE_H_


  		




  
  29
  
    

  		




  
  30
  
    
#include "humotion_yarp_icub/icub_jointinterface.h"


  		




  
  31
  
    

  		




  28
  32
  
    
#include <yarp/dev/ControlBoardInterfaces.h>


  		




  29
  33
  
    
#include <yarp/os/Network.h>


  		




  30
  
  
    
#include "icub_jointinterface.h"


  		




  31
  
  
    
/*


  		




  32
  
  
    
 * #include <humotion/server/server.h>


  		




  33
  
  
    
#include <yarp/os/RateThread.h>


  		




  34
  
  
    
#include <yarp/os/Time.h>


  		




  35
  
  
    
#include <yarp/os/Property.h>


  		




  36
  
  
    
#include <yarp/os/Port.h>


  		




  37
  
  
    
#include <yarp/dev/ControlBoardInterfaces.h>


  		




  38
  
  
    
#include "icub_data_receiver.h"


  		




  39
  
  
    
*/


  		




  40
  34
  
    

  		




  
  35
  
    
#include <string>


  		




  41
  36
  
    

  		




  42
  37
  
    
class iCubFaceInterface {


  		




  43
  
  
    
public:


  		




  44
  
  
    
    iCubFaceInterface(std::string scope);


  		




  
  38
  
    
 public:


  		




  
  39
  
    
    explicit iCubFaceInterface(std::string scope);


  		




  45
  40
  
    
    ~iCubFaceInterface();


  		




  46
  41
  
    

  		




  47
  42
  
    

  		




  ......




  49
  44
  
    
    void set_eyebrow_angle(int id, float *target_angle);


  		




  50
  45
  
    
    void set_mouth(float *target_angle);


  		




  51
  46
  
    

  		




  52
  
  
    
private:


  		




  
  47
  
    
 private:


  		




  53
  48
  
    
    double lid_angle;


  		




  54
  49
  
    
    int lid_opening_previous;


  		




  55
  50
  
    
    int previous_mouth_state;


  		




  56
  51
  
    
    double target_angle_previous[iCubJointInterface::ICUB_JOINT_ID_ENUM_SIZE];


  		




  57
  52
  
    
    std::string scope;


  		




  58
  53
  
    
    yarp::os::BufferedPort<yarp::os::Bottle> emotion_port[4];


  		




  59
  
  
    

  		




  60
  54
  
    
};


  		




  
  55
  
    

  		




  
  56
  
    
#endif  // EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_FACEINTERFACE_H_


  		












  

    
      examples/humotion_yarp_icub/include/humotion_yarp_icub/icub_jointinterface.h
    
  





  25
  25
  
    
* Excellence Initiative.


  		




  26
  26
  
    
*/


  		




  27
  27
  
    

  		




  28
  
  
    
#pragma once


  		




  
  28
  
    
#ifndef EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_JOINTINTERFACE_H_


  		




  
  29
  
    
#define EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_JOINTINTERFACE_H_


  		




  
  30
  
    

  		




  
  31
  
    
#include "humotion_yarp_icub/icub_data_receiver.h"


  		




  
  32
  
    

  		




  29
  33
  
    
#include <humotion/server/joint_interface.h>


  		




  
  34
  
    
#include <humotion/server/server.h>


  		




  
  35
  
    

  		




  30
  36
  
    
#include <boost/bimap.hpp>


  		




  31
  
  
    
#include <yarp/dev/PolyDriver.h>


  		




  32
  
  
    
#include <yarp/dev/IControlMode.h>


  		




  33
  
  
    
#include <yarp/dev/IControlLimits2.h>


  		




  34
  37
  
    
#include <yarp/dev/ControlBoardInterfaces.h>


  		




  35
  
  
    
#include <yarp/os/Time.h>


  		




  36
  
  
    
#include <yarp/sig/Vector.h>


  		




  37
  
  
    
#include "icub_jointinterface.h"


  		




  38
  
  
    
#include <humotion/server/server.h>


  		




  
  38
  
    
#include <yarp/dev/IControlLimits2.h>


  		




  
  39
  
    
#include <yarp/dev/IControlMode.h>


  		




  
  40
  
    
#include <yarp/dev/PolyDriver.h>


  		




  39
  41
  
    
#include <yarp/os/Network.h>


  		




  
  42
  
    
#include <yarp/os/Port.h>


  		




  
  43
  
    
#include <yarp/os/Property.h>


  		




  40
  44
  
    
#include <yarp/os/RateThread.h>


  		




  41
  45
  
    
#include <yarp/os/Time.h>


  		




  42
  
  
    
#include <yarp/os/Property.h>


  		




  43
  
  
    
#include <yarp/os/Port.h>


  		




  44
  
  
    
#include <yarp/dev/ControlBoardInterfaces.h>


  		




  45
  
  
    
#include "icub_data_receiver.h"


  		




  
  46
  
    
#include <yarp/sig/Vector.h>


  		




  
  47
  
    

  		




  
  48
  
    
#include <string>


  		




  
  49
  
    
#include <vector>


  		




  46
  50
  
    

  		




  47
  51
  
    
class iCubDataReceiver;


  		




  48
  52
  
    
class iCubFaceInterface;


  		




  49
  53
  
    

  		




  50
  54
  
    
class iCubJointInterface : public humotion::server::JointInterface{


  		




  51
  
  
    
public:


  		




  52
  
  
    
    iCubJointInterface(std::string scope);


  		




  
  55
  
    
 public:


  		




  
  56
  
    
    explicit iCubJointInterface(std::string scope);


  		




  53
  57
  
    
    ~iCubJointInterface();


  		




  54
  58
  
    

  		




  55
  
  
    
    //void store_incoming_position(int humotion_id, double position, double timestamp);


  		




  56
  
  
    
    //void store_incoming_velocity(int humotion_id, double position, double timestamp);


  		




  57
  59
  
    
    void run();


  		




  58
  60
  
    

  		




  59
  61
  
    
    int convert_icub_jointid_to_humotion(int id);


  		




  ......




  85
  87
  
    

  		




  86
  88
  
    
    static const int MAIN_LOOP_FREQUENCY = 50;


  		




  87
  89
  
    

  		




  88
  
  
    
protected:


  		




  
  90
  
    
 protected:


  		




  89
  91
  
    
    void disable_joint(int e);


  		




  90
  92
  
    
    void publish_target(int e, float position, float velocity);


  		




  91
  93
  
    
    void enable_joint(int e);


  		




  92
  94
  
    
    void execute_motion();


  		




  93
  95
  
    

  		




  94
  
  
    
private:


  		




  
  96
  
    
 private:


  		




  95
  97
  
    
    yarp::dev::PolyDriver yarp_polydriver_;


  		




  96
  98
  
    

  		




  97
  99
  
    
    std::vector<double> pv_mix_last_error_;


  		




  ......




  100
  102
  
    

  		




  101
  103
  
    
    // yarp views


  		




  102
  104
  
    
    yarp::dev::IVelocityControl *yarp_ivel_;


  		




  103
  
  
    
    //yarp::dev::IPositionControl *yarp_ipos_;


  		




  104
  
  
    

  		




  105
  
  
    
    //yarp::dev::IEncodersTimed *yarp_iencs_;


  		




  106
  105
  
    
    yarp::dev::IControlLimits *yarp_ilimits_;


  		




  107
  106
  
    
    yarp::dev::IAmplifierControl *yarp_amp_;


  		




  108
  107
  
    
    yarp::dev::IPidControl *yarp_pid_;


  		




  ......




  122
  121
  
    

  		




  123
  122
  
    
    void store_icub_joint_target(int icub_id, float position, float velocity);


  		




  124
  123
  
    

  		




  125
  
  
    
    //*******************************************


  		




  126
  
  
    

  		




  127
  124
  
    

  		




  128
  125
  
    
    void set_joint_enable_state(int e, bool enabled);


  		




  129
  
  
    
    //double target_angle[ICUB_JOINT_ID_ENUM_SIZE];


  		




  130
  
  
    
    //double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];


  		




  131
  126
  
    

  		




  132
  127
  
    
    iCubDataReceiver *icub_data_receiver;


  		




  133
  128
  
    
    void init_joints();


  		




  134
  129
  
    

  		




  135
  130
  
    
    std::string scope;


  		




  136
  
  
    
    //yarp::sig::Vector positions;


  		




  137
  
  
    
    //yarp::sig::Vector velocities;


  		




  138
  
  
    

  		




  139
  131
  
    

  		




  140
  132
  
    
    void store_min_max(yarp::dev::IControlLimits *ilimits, int icub_id);


  		




  141
  133
  
    

  		




  142
  
  
    
   //float last_pos_eye_vergence;


  		




  143
  
  
    
    //float last_pos_eye_pan;


  		




  144
  
  
    
   // float last_vel_eye_vergence;


  		




  145
  
  
    
    //float last_vel_eye_pan;


  		




  146
  
  
    

  		




  147
  
  
    
    //void store_joint(int id, float value);


  		




  148
  134
  
    
    void set_target_in_velocitymode(int id);


  		




  149
  135
  
    

  		




  150
  
  
    

  		




  151
  
  
    
    //int convert_enum_to_motorid(int e);


  		




  152
  
  
    
    //int convert_motorid_to_enum(int id);


  		




  153
  
  
    

  		




  154
  136
  
    
    iCubFaceInterface *face_interface_;


  		




  155
  137
  
    

  		




  156
  138
  
    
    typedef boost::bimap<int, int > enum_id_bimap_t;


  		




  157
  139
  
    
    typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;


  		




  158
  140
  
    
    enum_id_bimap_t enum_id_bimap;


  		




  159
  141
  
    
};


  		




  
  142
  
    

  		




  
  143
  
    
#endif  // EXAMPLES_HUMOTION_YARP_ICUB_INCLUDE_HUMOTION_YARP_ICUB_ICUB_JOINTINTERFACE_H_


  		












  

    
      examples/humotion_yarp_icub/src/icub_data_receiver.cpp
    
  





  29
  29
  
    
#include <humotion/server/joint_interface.h>


  		




  30
  30
  
    
#include <yarp/os/Property.h>


  		




  31
  31
  
    

  		




  
  32
  
    
#include <string>


  		




  
  33
  
    

  		




  32
  34
  
    
#include "humotion_yarp_icub/icub_data_receiver.h"


  		




  33
  35
  
    

  		




  34
  36
  
    
using std::cout;


  		




  ......




  143
  145
  
    
//! \param position


  		




  144
  146
  
    
//! \param timestamp


  		




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


  		




  146
  
  
    
    //cout << "store_incoming_position(icub=" << icub_id << ", " << position << ")\n";


  		




  
  148
  
    
    // cout << "store_incoming_position(icub=" << icub_id << ", " << position << ")\n";


  		




  147
  149
  
    

  		




  148
  150
  
    
    // store joint position in humotion backend


  		




  149
  151
  
    
    if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||


  		




  ......




  166
  168
  
    
                                                     right, timestamp);


  		




  167
  169
  
    
    } else if (icub_id == 100) {


  		




  168
  170
  
    
        //HACK


  		




  169
  
  
    
        //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,


  		




  
  171
  
    
        // icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,


  		




  170
  172
  
    
        //                                             lid_angle, timestamp);


  		




  171
  173
  
    
    } else {


  		




  172
  174
  
    
        if (icub_id == iCubJointInterface::ID_NECK_PAN) {


  		




  ......




  185
  187
  
    
//! \param velocity


  		




  186
  188
  
    
//! \param timestamp


  		




  187
  189
  
    
void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, double timestamp) {


  		




  188
  
  
    
    //cout << "store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";


  		




  
  190
  
    
    // cout << "store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";


  		




  189
  191
  
    

  		




  190
  192
  
    
    // store joint position in humotion backend


  		




  191
  193
  
    
    if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||


  		




  ......




  208
  210
  
    
                                                     right, timestamp);


  		




  209
  211
  
    
    } else if (icub_id == 100) {


  		




  210
  212
  
    
        //HACK


  		




  211
  
  
    
        //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,


  		




  
  213
  
    
        // icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,


  		




  212
  214
  
    
        //                                             lid_angle, timestamp);


  		




  213
  215
  
    
    } else {


  		




  214
  216
  
    
        if (icub_id == iCubJointInterface::ID_NECK_PAN) {


  		




  ......




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


  		




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


  		




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


  		




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


  		




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


  		




  233
  235
  
    
    cout << "\n";


  		




  234
  236
  
    
            // publish data to humotion


  		




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


  		




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


  		




  236
  238
  
    
        cout << positions_[i] << " ";


  		




  237
  239
  
    
        cout << velocities_[i] << " ";


  		




  238
  240
  
    
    }


  		












  

    
      examples/humotion_yarp_icub/src/icub_faceinterface.cpp
    
  





  27
  27
  
    

  		




  28
  28
  
    
#include "humotion_yarp_icub/icub_faceinterface.h"


  		




  29
  29
  
    

  		




  
  30
  
    
#include <boost/format.hpp>


  		




  
  31
  
    

  		




  30
  32
  
    
#include <algorithm>


  		




  31
  33
  
    
#include <string>


  		




  32
  
  
    
#include <boost/format.hpp>


  		




  33
  34
  
    

  		




  34
  35
  
    
using yarp::os::Network;


  		




  35
  36
  
    
using yarp::os::Bottle;


  		












  

    
      examples/humotion_yarp_icub/src/main.cpp
    
  





  42
  42
  
    
#include "humotion_yarp_icub/icub_jointinterface.h"


  		




  43
  43
  
    

  		




  44
  44
  
    
using std::cerr;


  		




  
  45
  
    
using yarp::os::Network;


  		




  
  46
  
    
using yarp::os::Property;


  		




  45
  47
  
    

  		




  46
  48
  
    
// using namespace yarp::dev;


  		




  47
  49
  
    
// using namespace yarp::sig;


  		




  ......




  66
  68
  
    
        return EXIT_FAILURE;


  		




  67
  69
  
    
    }


  		




  68
  70
  
    

  		




  69
  
  
    
    string robotName = params.find("robot").asString().c_str();


  		




  70
  
  
    
    string scope = "/" + robotName;


  		




  
  71
  
    
    std::string robotName = params.find("robot").asString().c_str();


  		




  
  72
  
    
    std::string scope = "/" + robotName;


  		




  71
  73
  
    

  		




  72
  74
  
    

  		




  73
  75
  
    
    // create humotion interface


  		












Also available in:  Unified diff