Revision 35b3ca25 examples/yarp_icub/include/icub_jointinterface.h

View differences:

examples/yarp_icub/include/icub_jointinterface.h
25 25 
    iCubJointInterface(std::string scope);
26 26 
    ~iCubJointInterface();
27 27 

  		




  28
  
  
    
    //void fetch_position(Device *dev, double timestamp);


  		




  29
  
  
    
    //void fetch_speed(Device *dev, double timestamp);


  		




  30
  
  
    
    void fetch_position(int id, double value, double timestamp);


  		




  31
  
  
    
    void fetch_speed(int id, double value, double timestamp);


  		




  
  28
  
    
    void store_incoming_position(int id, double value, double timestamp);


  		




  32
  29
  
    
    void run();


  		




  33
  30
  
    

  		




  
  31
  
    
    int convert_icub_jointid_to_humotion(int id);


  		




  
  32
  
    
    int convert_humotion_jointid_to_icub(int id);


  		




  
  33
  
    

  		




  34
  34
  
    
    enum JOINT_ID_ENUM{


  		




  35
  35
  
    
        ICUB_ID_NECK_TILT = 0,


  		




  36
  36
  
    
        ICUB_ID_NECK_ROLL = 1,


  		




  ......




  38
  38
  
    
        ICUB_ID_EYES_BOTH_UD = 3,


  		




  39
  39
  
    
        ICUB_ID_EYES_PAN = 4,


  		




  40
  40
  
    
        ICUB_ID_EYES_VERGENCE = 5,


  		




  41
  
  
    
        //the following ids are used for remapping:


  		




  42
  
  
    
        //ICUB_ID_EYES_LEFT_LR,


  		




  43
  
  
    
        //ICUB_ID_EYES_RIGHT_LR,


  		




  44
  41
  
    
        ICUB_ID_EYES_LEFT_LID_LOWER,


  		




  45
  42
  
    
        ICUB_ID_EYES_LEFT_LID_UPPER,


  		




  46
  43
  
    
        ICUB_ID_EYES_RIGHT_LID_LOWER,


  		




  ......




  56
  53
  
    
        ICUB_JOINT_ID_ENUM_SIZE


  		




  57
  54
  
    
    };


  		




  58
  55
  
    

  		




  
  56
  
    
    yarp::dev::PolyDriver *get_yarp_polydriver() { return &yarp_polydriver_; }


  		




  
  57
  
    

  		




  59
  58
  
    
    static const int MAIN_LOOP_FREQUENCY = 50;


  		




  60
  59
  
    

  		




  61
  60
  
    
protected:


  		




  62
  61
  
    
    void disable_joint(int e);


  		




  63
  
  
    
    void publish_target_position(int e);


  		




  
  62
  
    
    void publish_target(int e, float position, float velocity);


  		




  64
  63
  
    
    void enable_joint(int e);


  		




  65
  64
  
    
    void execute_motion();


  		




  66
  65
  
    

  		




  67
  66
  
    
private:


  		




  
  67
  
    
    yarp::dev::PolyDriver yarp_polydriver_;


  		




  
  68
  
    

  		




  
  69
  
    
    std::vector<double> pv_mix_last_error_;


  		




  
  70
  
    
    std::vector<double> pv_mix_pid_p_;


  		




  
  71
  
    
    std::vector<double> pv_mix_pid_d_;


  		




  
  72
  
    

  		




  
  73
  
    
    // yarp views


  		




  
  74
  
    
    yarp::dev::IVelocityControl *yarp_ivel_;


  		




  
  75
  
    
    yarp::dev::IPositionControl *yarp_ipos_;


  		




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


  		




  
  77
  
    
    yarp::dev::IControlLimits *yarp_ilimits_;


  		




  
  78
  
    
    yarp::dev::IAmplifierControl *yarp_amp_;


  		




  
  79
  
    
    yarp::dev::IPidControl *yarp_pid_;


  		




  
  80
  
    
    yarp::dev::IControlMode *yarp_icontrol_;


  		




  
  81
  
    

  		




  
  82
  
    
    yarp::sig::Vector yarp_commands_;


  		




  
  83
  
    

  		




  
  84
  
    
    float target_angle_[ICUB_JOINT_ID_ENUM_SIZE];


  		




  
  85
  
    
    float target_velocity_[ICUB_JOINT_ID_ENUM_SIZE];


  		




  
  86
  
    

  		




  
  87
  
    

  		




  
  88
  
    
    void init_controller();


  		




  
  89
  
    
    void init_id_map();


  		




  
  90
  
    
    void init_pv_mix_pid();


  		




  
  91
  
    
    void insert_icupid_to_humotionid_mapping(int icubid, int humotionid);


  		




  
  92
  
    

  		




  
  93
  
    

  		




  
  94
  
    
    void store_icub_joint_target(int icub_id, float position, float velocity);


  		




  
  95
  
    

  		




  
  96
  
    
    //*******************************************


  		




  
  97
  
    

  		




  
  98
  
    

  		




  68
  99
  
    
    void set_joint_enable_state(int e, bool enabled);


  		




  69
  
  
    
    double get_timestamp_ms();


  		




  70
  
  
    
    double target_angle[ICUB_JOINT_ID_ENUM_SIZE];


  		




  71
  
  
    
    double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];


  		




  
  100
  
    
    //double target_angle[ICUB_JOINT_ID_ENUM_SIZE];


  		




  
  101
  
    
    //double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];


  		




  72
  102
  
    

  		




  73
  103
  
    
    iCubDataReceiver *icub_data_receiver;


  		




  74
  104
  
    
    void init_joints();


  		




  75
  105
  
    

  		




  76
  106
  
    
    std::string scope;


  		




  77
  
  
    
    yarp::dev::PolyDriver dd;


  		




  78
  107
  
    
    yarp::sig::Vector positions;


  		




  79
  108
  
    
    yarp::sig::Vector velocities;


  		




  80
  
  
    
    yarp::sig::Vector commands;


  		




  81
  
  
    

  		




  82
  
  
    
    yarp::dev::IVelocityControl *ivel;


  		




  83
  
  
    
    yarp::dev::IPositionControl *ipos;


  		




  84
  
  
    
    yarp::dev::IEncodersTimed *iencs;


  		




  85
  
  
    
    yarp::dev::IControlLimits *ilimits;


  		




  86
  
  
    
    yarp::dev::IAmplifierControl *amp;


  		




  87
  
  
    
    yarp::dev::IPidControl *pid;


  		




  88
  
  
    
    yarp::dev::IControlMode *icontrol;


  		




  89
  
  
    
    std::vector<double> last_position_error;


  		




  90
  
  
    
    std::vector<double> PID_P;


  		




  91
  
  
    
    std::vector<double> PID_D;


  		




  
  109
  
    

  		




  92
  110
  
    

  		




  93
  111
  
    
    void store_min_max(yarp::dev::IControlLimits *ilimits, int id, int e);


  		




  94
  112
  
    

  		




  ......




  105
  123
  
    
    int convert_enum_to_motorid(int e);


  		




  106
  124
  
    
    int convert_motorid_to_enum(int id);


  		




  107
  125
  
    

  		




  108
  
  
    
    iCubFaceInterface *face_interface;


  		




  
  126
  
    
    iCubFaceInterface *face_interface_;


  		




  109
  127
  
    

  		




  110
  128
  
    
    typedef boost::bimap<int, int > enum_id_bimap_t;


  		




  111
  129
  
    
    typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;


  		












Also available in:  Unified diff