Revision 10cae485

View differences:

examples/meka/include/mekajointinterface.h
19 19 
    //void fetch_speed(int id, double value, double timestamp);
20 20 
    void run();
21 21 

  		




  22
  
  
    
    enum JOINT_ID_ENUM{


  		




  23
  
  
    
        ICUB_ID_NECK_TILT = 0,


  		




  24
  
  
    
        ICUB_ID_NECK_ROLL = 1,


  		




  25
  
  
    
        ICUB_ID_NECK_PAN  = 2,


  		




  26
  
  
    
        ICUB_ID_EYES_BOTH_UD = 3,


  		




  27
  
  
    
        ICUB_ID_EYES_PAN = 4,


  		




  28
  
  
    
        ICUB_ID_EYES_VERGENCE = 5,


  		




  29
  
  
    
        //the following ids are used for remapping:


  		




  30
  
  
    
        ICUB_ID_EYES_LEFT_LR,


  		




  31
  
  
    
        ICUB_ID_EYES_RIGHT_LR,


  		




  32
  
  
    
        ICUB_ID_EYES_LEFT_LID_LOWER,


  		




  33
  
  
    
        ICUB_ID_EYES_LEFT_LID_UPPER,


  		




  34
  
  
    
        ICUB_ID_EYES_RIGHT_LID_LOWER,


  		




  35
  
  
    
        ICUB_ID_EYES_RIGHT_LID_UPPER,


  		




  36
  
  
    
        ICUB_ID_EYES_LEFT_BROW,


  		




  37
  
  
    
        ICUB_ID_EYES_RIGHT_BROW,


  		




  38
  
  
    
        ICUB_ID_LIP_LEFT_UPPER,


  		




  39
  
  
    
        ICUB_ID_LIP_LEFT_LOWER,


  		




  40
  
  
    
        ICUB_ID_LIP_CENTER_UPPER,


  		




  41
  
  
    
        ICUB_ID_LIP_CENTER_LOWER,


  		




  42
  
  
    
        ICUB_ID_LIP_RIGHT_UPPER,


  		




  43
  
  
    
        ICUB_ID_LIP_RIGHT_LOWER,


  		




  44
  
  
    
        ICUB_JOINT_ID_ENUM_SIZE


  		




  45
  
  
    
    };


  		




  46
  
  
    

  		




  47
  22
  
    
    static const int MAIN_LOOP_FREQUENCY = 50;


  		




  48
  23
  
    

  		




  49
  24
  
    
protected:


  		




  ......




  59
  34
  
    
    ros::Subscriber joint_state_subscriber;


  		




  60
  35
  
    
    ros::Publisher target_publisher;


  		




  61
  36
  
    

  		




  62
  
  
    

  		




  63
  37
  
    
    double get_timestamp_s();


  		




  64
  
  
    
    double target_angle[ICUB_JOINT_ID_ENUM_SIZE];


  		




  65
  
  
    
    double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];


  		




  66
  38
  
    

  		




  67
  39
  
    
    void set_eyelid_angle(double angle);


  		




  68
  40
  
    
    void set_eyebrow_angle(int id);


  		












  

    
      examples/meka/src/mekajointinterface.cpp
    
  





  124
  124
  
    

  		




  125
  125
  
    
    trajectory_msgs::JointTrajectoryPoint p;


  		




  126
  126
  
    
    p.positions.push_back(joint_target[ID_NECK_TILT] * M_PI / 180.0);


  		




  127
  
  
    
    p.positions.push_back(joint_target[ID_NECK_PAN] * M_PI / 180.0);


  		




  
  127
  
    
    p.positions.push_back(joint_target[ID_NECK_PAN] * M_PI / 180.0 +0.314);


  		




  
  128
  
    
    printf("targets pan=%4.1f tilt=%4.1f (eye p %4.1f t %4.2f)\n",joint_target[ID_NECK_TILT],joint_target[ID_NECK_PAN],joint_target[ID_EYES_LEFT_LR],joint_target[ID_EYES_BOTH_UD]);


  		




  128
  129
  
    

  		




  129
  130
  
    
    p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);


  		




  130
  131
  
    

  		




  ......




  202
  203
  
    
}


  		




  203
  204
  
    

  		




  204
  205
  
    
void MekaJointInterface::init_joints(){


  		




  205
  
  
    
    store_min_max(ID_NECK_TILT, -10, 10);


  		




  206
  
  
    
    store_min_max(ID_NECK_PAN, -10, 10);


  		




  
  206
  
    
    store_min_max(ID_NECK_TILT, -18, 1);


  		




  
  207
  
    
    store_min_max(ID_NECK_PAN, -56, 56);


  		




  207
  208
  
    

  		




  208
  209
  
    
    store_min_max(ID_NECK_ROLL, -1, 1);


  		




  209
  210
  
    
    store_min_max(ID_EYES_BOTH_UD, -1, 1);


  		












  

    
      src/server/controller.cpp
    
  





  131
  131
  
    
void Controller::set_gaze_target(GazeState new_gaze_target){


  		




  132
  132
  
    
    if (!activated){


  		




  133
  133
  
    
        //not yet initialized, ignore incoming targets


  		




  
  134
  
    
	printf("not activated\n");


  		




  134
  135
  
    
        return;


  		




  135
  136
  
    
    }


  		




  136
  137
  
    

  		




  137
  138
  
    
    GazeState target_gaze;


  		




  
  139
  
    
    new_gaze_target.dump();


  		




  138
  140
  
    

  		




  139
  141
  
    
    //relative or absolute gaze update?


  		




  140
  142
  
    
    if (new_gaze_target.gaze_type == GazeState::GAZETYPE_RELATIVE){


  		












  

    
      src/server/eye_motion_generator.cpp
    
  





  86
  86
  
    
        eye_tilt_target  = 0.0;


  		




  87
  87
  
    
    }


  		




  88
  88
  
    

  		




  
  89
  
    
    //check and take care of limits


  		




  
  90
  
    
    eye_pan_l_target = limit_target(JointInterface::ID_EYES_LEFT_LR, eye_pan_l_target);


  		




  
  91
  
    
    eye_pan_r_target = limit_target(JointInterface::ID_EYES_RIGHT_LR, eye_pan_r_target);


  		




  
  92
  
    
    eye_tilt_target = limit_target(JointInterface::ID_EYES_BOTH_UD, eye_tilt_target);


  		




  
  93
  
    

  		




  89
  94
  
    
    //fetch current angles:


  		




  90
  95
  
    
    float eye_pan_l_now = get_current_position(JointInterface::ID_EYES_LEFT_LR);


  		




  91
  96
  
    
    float eye_pan_r_now = get_current_position(JointInterface::ID_EYES_RIGHT_LR);


  		












Also available in:  Unified diff