Revision 6d13138a src/server/gaze_motion_generator.cpp

View differences:

src/server/gaze_motion_generator.cpp
29 29 
#include "humotion/server/server.h"
30 30 

  		




  31
  31
  
    
using humotion::server::GazeMotionGenerator;


  		




  32
  
  
    

  		




  33
  
  
    
// saccade detection threshold in deg/s


  		




  34
  
  
    
// const float GazeMotionGenerator::SACCADE_SPEED_THRESHOLD = 15.0;


  		




  35
  
  
    
// some constants to decide if the requested move will trigger a neck or eye saccade


  		




  36
  
  
    
const float GazeMotionGenerator::NECK_SACCADE_THRESHOLD      = 15.0;  // deg


  		




  37
  
  
    
const float GazeMotionGenerator::EYE_SACCADE_SPEED_THRESHOLD = 15.0;  // deg/s


  		




  38
  
  
    

  		




  39
  
  
    
// close to ocolumotor-range?


  		




  40
  
  
    
// 0.95 = reaching 95% of OMR will trigger a correction


  		




  41
  
  
    
const float GazeMotionGenerator::OMR_LIMIT_TRIGGERS_NECK_SACCADE = 0.95;


  		




  42
  
  
    

  		




  
  32
  
    
using humotion::server::Config;


  		




  43
  33
  
    

  		




  44
  34
  
    
//! constructor


  		




  45
  
  
    
GazeMotionGenerator::GazeMotionGenerator(JointInterface *j, int dof, float t)


  		




  46
  
  
    
    : ReflexxesMotionGenerator(j, dof, t) {


  		




  
  35
  
    
GazeMotionGenerator::GazeMotionGenerator(JointInterface *j, Config *cfg, int dof, float t)


  		




  
  36
  
    
    : ReflexxesMotionGenerator(j, cfg, dof, t) {


  		




  47
  37
  
    
}


  		




  48
  38
  
    

  		




  49
  39
  
    
//! destructor


  		




  ......




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


  		




  67
  57
  
    

  		




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


  		




  69
  
  
    
    if (dist > NECK_SACCADE_THRESHOLD) {


  		




  
  59
  
    
    if (dist > config->threshold_angle_neck_saccade) {


  		




  70
  60
  
    
        // the next saccade has to use neck motion as well


  		




  71
  
  
    
        if (speed > EYE_SACCADE_SPEED_THRESHOLD) {


  		




  
  61
  
    
        if (speed > config->threshold_velocity_eye_saccade) {


  		




  72
  62
  
    
            neck_saccade_requested = true;


  		




  73
  63
  
    
        }


  		




  74
  64
  
    
    } else {


  		




  ......




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


  		




  84
  74
  
    

  		




  85
  75
  
    
    // min/max bounds


  		




  86
  
  
    
    float left_min  = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  
  76
  
    
    float left_min  = config->threshold_angle_omr_limit *


  		




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


  		




  88
  
  
    
    float left_max  = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  
  78
  
    
    float left_max  = config->threshold_angle_omr_limit *


  		




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


  		




  90
  
  
    
    float right_min = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  
  80
  
    
    float right_min = config->threshold_angle_omr_limit *


  		




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


  		




  92
  
  
    
    float right_max = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  
  82
  
    
    float right_max = config->threshold_angle_omr_limit *


  		




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


  		




  94
  
  
    
    float ud_min    = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  
  84
  
    
    float ud_min    = config->threshold_angle_omr_limit *


  		




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


  		




  96
  
  
    
    float ud_max    = OMR_LIMIT_TRIGGERS_NECK_SACCADE *


  		




  
  86
  
    
    float ud_max    = config->threshold_angle_omr_limit *


  		




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


  		




  98
  88
  
    

  		




  99
  89
  
    
    if (


  		




  ......




  125
  115
  
    
    float speed_total_r = sqrt(speed_right*speed_right + speed_tilt*speed_tilt);


  		




  126
  116
  
    

  		




  127
  117
  
    
    // thresholding


  		




  128
  
  
    
    if ((speed_total_l > EYE_SACCADE_SPEED_THRESHOLD) ||


  		




  129
  
  
    
            (speed_total_r > EYE_SACCADE_SPEED_THRESHOLD)) {


  		




  
  118
  
    
    if ((speed_total_l > config->threshold_velocity_eye_saccade) ||


  		




  
  119
  
    
            (speed_total_r > config->threshold_velocity_eye_saccade)) {


  		




  130
  120
  
    
        // this is a saccade


  		




  131
  121
  
    
        saccade_active = true;


  		




  132
  122
  
    
    } else {


  		












Also available in:  Unified diff