Revision ea068cf1 src/server/gaze_motion_generator.cpp

View differences:

src/server/gaze_motion_generator.cpp
53 53 
//! update gaze target:
54 54 
//! \param GazeState with target values for the overall gaze
55 55 
void GazeMotionGenerator::set_gaze_target(GazeState new_gaze_target) {
56 
    if (requested_gaze_state.gaze_type == GazeState::GAZETYPE_RELATIVE) {
56 
    if (requested_gaze_state_.gaze_type == GazeState::GAZETYPE_RELATIVE) {
57 57 
        printf("> ERROR: gaze targets should be converted to absolute before calling this\n");
58 58 
        exit(EXIT_FAILURE);
59 59 
    }
60 60 

  		




  61
  61
  
    
    // check magnitude of gaze change to detect eye-neck saccades


  		




  62
  
  
    
    float dist = fabs(requested_gaze_state.distance_pt_abs(new_gaze_target));


  		




  
  62
  
    
    float dist = fabs(requested_gaze_state_.distance_pt_abs(new_gaze_target));


  		




  63
  63
  
    

  		




  64
  64
  
    
    // check requested speed


  		




  65
  
  
    
    float speed = fabs(requested_gaze_state.distance_pt_abs(new_gaze_target))


  		




  66
  
  
    
            / (new_gaze_target.timestamp.to_seconds()-requested_gaze_state.timestamp.to_seconds());


  		




  
  65
  
    
    float speed = fabs(requested_gaze_state_.distance_pt_abs(new_gaze_target))


  		




  
  66
  
    
            / (new_gaze_target.timestamp.to_seconds()-requested_gaze_state_.timestamp.to_seconds());


  		




  67
  67
  
    

  		




  68
  68
  
    
    // check magnitude and speed of gaze change to detect eye-neck saccades


  		




  69
  69
  
    
    if (dist > NECK_SACCADE_THRESHOLD) {


  		




  ......




  78
  78
  
    
    // check for eye getting close to ocolomotor range


  		




  79
  79
  
    
    // actually it makes more sense to trigger the neck saccade based on


  		




  80
  80
  
    
    // the target getting out of the OMR


  		




  81
  
  
    
    float eye_target_l  = joint_interface->get_target_position(JointInterface::ID_EYES_LEFT_LR);


  		




  82
  
  
    
    float eye_target_r  = joint_interface->get_target_position(JointInterface::ID_EYES_RIGHT_LR);


  		




  83
  
  
    
    float eye_target_ud = joint_interface->get_target_position(JointInterface::ID_EYES_BOTH_UD);


  		




  
  81
  
    
    float eye_target_l  = joint_interface_->get_target_position(JointInterface::ID_EYES_LEFT_LR);


  		




  
  82
  
    
    float eye_target_r  = joint_interface_->get_target_position(JointInterface::ID_EYES_RIGHT_LR);


  		




  
  83
  
    
    float eye_target_ud = joint_interface_->get_target_position(JointInterface::ID_EYES_BOTH_UD);


  		




  84
  84
  
    

  		




  85
  85
  
    
    // min/max bounds


  		




  86
  86
  
    
    float left_min  = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  87
  
  
    
            joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LR);


  		




  
  87
  
    
            joint_interface_->get_joint_min(JointInterface::ID_EYES_LEFT_LR);


  		




  88
  88
  
    
    float left_max  = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  89
  
  
    
            joint_interface->get_joint_max(JointInterface::ID_EYES_LEFT_LR);


  		




  
  89
  
    
            joint_interface_->get_joint_max(JointInterface::ID_EYES_LEFT_LR);


  		




  90
  90
  
    
    float right_min = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  91
  
  
    
            joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LR);


  		




  
  91
  
    
            joint_interface_->get_joint_min(JointInterface::ID_EYES_RIGHT_LR);


  		




  92
  92
  
    
    float right_max = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  93
  
  
    
            joint_interface->get_joint_max(JointInterface::ID_EYES_RIGHT_LR);


  		




  
  93
  
    
            joint_interface_->get_joint_max(JointInterface::ID_EYES_RIGHT_LR);


  		




  94
  94
  
    
    float ud_min    = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  95
  
  
    
            joint_interface->get_joint_min(JointInterface::ID_EYES_BOTH_UD);


  		




  
  95
  
    
            joint_interface_->get_joint_min(JointInterface::ID_EYES_BOTH_UD);


  		




  96
  96
  
    
    float ud_max    = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  97
  
  
    
            joint_interface->get_joint_max(JointInterface::ID_EYES_BOTH_UD);


  		




  
  97
  
    
            joint_interface_->get_joint_max(JointInterface::ID_EYES_BOTH_UD);


  		




  98
  98
  
    

  		




  99
  99
  
    
    if (


  		




  100
  100
  
    
            (eye_target_l < left_min) || (eye_target_l > left_max) ||


  		




  ......




  114
  114
  
    
bool GazeMotionGenerator::get_eye_saccade_active() {


  		




  115
  115
  
    
    bool saccade_active;


  		




  116
  116
  
    

  		




  117
  
  
    
    float speed_left  = joint_interface->get_ts_speed(


  		




  
  117
  
    
    float speed_left  = joint_interface_->get_ts_speed(


  		




  118
  118
  
    
                JointInterface::ID_EYES_LEFT_LR).get_newest_value();


  		




  119
  
  
    
    float speed_right = joint_interface->get_ts_speed(


  		




  
  119
  
    
    float speed_right = joint_interface_->get_ts_speed(


  		




  120
  120
  
    
                JointInterface::ID_EYES_RIGHT_LR).get_newest_value();


  		




  121
  
  
    
    float speed_tilt  = joint_interface->get_ts_speed(


  		




  
  121
  
    
    float speed_tilt  = joint_interface_->get_ts_speed(


  		




  122
  122
  
    
                JointInterface::ID_EYES_BOTH_UD).get_newest_value();


  		




  123
  123
  
    

  		




  124
  124
  
    
    float speed_total_l = sqrt(speed_left*speed_left + speed_tilt*speed_tilt);


  		




  ......




  139
  139
  
    
//! get overall gaze


  		




  140
  140
  
    
humotion::GazeState GazeMotionGenerator::get_current_gaze() {


  		




  141
  141
  
    
    // shortcut, makes the following easier to read


  		




  142
  
  
    
    JointInterface *j = joint_interface;


  		




  
  142
  
    
    JointInterface *j = joint_interface_;


  		




  143
  143
  
    

  		




  144
  
  
    
    GazeState gaze = requested_gaze_state;


  		




  
  144
  
    
    GazeState gaze = requested_gaze_state_;


  		




  145
  145
  
    

  		




  146
  146
  
    
    gaze.pan = j->get_ts_position(JointInterface::ID_NECK_PAN).get_newest_value() +


  		




  147
  147
  
    
            (j->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_newest_value()


  		












Also available in:  Unified diff