Revision 0c8d22a5 src/server/eye_motion_generator.cpp

View differences:

src/server/eye_motion_generator.cpp
25 25 
* Excellence Initiative.
26 26 
*/
27 27 

  		




  28
  
  
    
#include "server/eye_motion_generator.h"


  		




  29
  
  
    
#include "server/server.h"


  		




  
  28
  
    
#include "humotion/server/eye_motion_generator.h"


  		




  
  29
  
    
#include "humotion/server/server.h"


  		




  30
  30
  
    

  		




  31
  
  
    
using namespace std;


  		




  32
  
  
    
using namespace humotion;


  		




  33
  
  
    
using namespace humotion::server;


  		




  
  31
  
    
// using namespace std;


  		




  
  32
  
    
// using namespace humotion;


  		




  
  33
  
    
// using namespace humotion::server;


  		




  34
  34
  
    

  		




  35
  
  
    
//saccade detection threshold in deg/s


  		




  
  35
  
    
using humotion::server::EyeMotionGenerator;


  		




  
  36
  
    

  		




  
  37
  
    
// saccade detection threshold in deg/s


  		




  36
  38
  
    
const float EyeMotionGenerator::SACCADE_SPEED_THRESHOLD = 15.0;


  		




  37
  39
  
    

  		




  38
  40
  
    
//! constructor


  		




  39
  
  
    
EyeMotionGenerator::EyeMotionGenerator(JointInterface *j) : GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE){


  		




  
  41
  
    
EyeMotionGenerator::EyeMotionGenerator(JointInterface *j) :


  		




  
  42
  
    
    GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE) {


  		




  40
  43
  
    
}


  		




  41
  44
  
    

  		




  42
  45
  
    

  		




  43
  46
  
    
//! destructor


  		




  44
  
  
    
EyeMotionGenerator::~EyeMotionGenerator(){


  		




  
  47
  
    
EyeMotionGenerator::~EyeMotionGenerator() {


  		




  45
  48
  
    
}


  		




  46
  49
  
    

  		




  47
  50
  
    
//! set up an eyemotion profile


  		




  ......




  51
  54
  
    
//! \param current angle


  		




  52
  55
  
    
void EyeMotionGenerator::setup_eyemotion(int dof, float target,


  		




  53
  56
  
    
                                         float current_position,


  		




  54
  
  
    
                                         float current_speed,


  		




  55
  
  
    
                                         humotion::Timestamp timestamp){


  		




  56
  
  
    
    //get distance to target:


  		




  
  57
  
    
                                         float current_velocity,


  		




  
  58
  
    
                                         humotion::Timestamp timestamp) {


  		




  
  59
  
    
    // get distance to target:


  		




  57
  60
  
    
    float distance_abs = fabs(target - current_position);


  		




  58
  
  
    
    //get max speed: factor can be found in encyc britannica: "linear.... being 300° per second for 10° and 500° per second for 30°" (max=700)


  		




  
  61
  
    
    // get max speed: factor can be found in encyc britannica:


  		




  
  62
  
    
    // "linear.... being 300° per second for 10° and 500° per second for 30°" (max=700)


  		




  59
  63
  
    
    float max_speed = fmin(700.0, 10.0*distance_abs + 200.0);


  		




  60
  
  
    
    //max accel: use data from ""Speed and Accuracy of Saccadic Eye Movements: Characteristics of Impulse Variability in the Oculomotor System"


  		




  61
  
  
    
    //                 http://www-personal.umich.edu/~kornblum/files/journal_exp_psych_HPP_15-3.pdf


  		




  62
  
  
    
    //           gathered from table2


  		




  
  64
  
    
    // max accel: use data from:


  		




  
  65
  
    
    // "Speed and Accuracy of Saccadic Eye Movements:


  		




  
  66
  
    
    //      Characteristics of Impulse Variability in the Oculomotor System"


  		




  
  67
  
    
    // http://www-personal.umich.edu/~kornblum/files/journal_exp_psych_HPP_15-3.pdf [table 2]


  		




  63
  68
  
    
    float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4);


  		




  64
  69
  
    

  		




  65
  
  
    
    //feed reflexxes api with data


  		




  66
  
  
    
    reflexxes_set_input(dof, target, current_position, current_speed, timestamp, max_speed, max_accel);


  		




  
  70
  
    
    // feed reflexxes api with data


  		




  
  71
  
    
    reflexxes_set_input(dof, target, current_position, current_velocity,


  		




  
  72
  
    
                        timestamp, max_speed, max_accel);


  		




  67
  73
  
    
}


  		




  68
  74
  
    

  		




  69
  75
  
    
//! calculate joint targets


  		




  70
  
  
    
void EyeMotionGenerator::calculate_targets(){


  		




  71
  
  
    
    //use the target values for a faster response


  		




  
  76
  
    
void EyeMotionGenerator::calculate_targets() {


  		




  
  77
  
    
    // use the target values for a faster response


  		




  72
  78
  
    
    float neck_pan_now  = joint_interface->get_target_position(JointInterface::ID_NECK_PAN);


  		




  73
  79
  
    
    float neck_tilt_now = joint_interface->get_target_position(JointInterface::ID_NECK_TILT);


  		




  74
  80
  
    

  		




  75
  
  
    
    //calculate target angles for the eyes:


  		




  76
  
  
    
    //right eye is dominant -> direct output


  		




  77
  
  
    
    float eye_pan_r_target = (requested_gaze_state.pan + requested_gaze_state.vergence/2.0) - (neck_pan_now);


  		




  
  81
  
    
    // calculate target angles for the eyes


  		




  
  82
  
    
    // right eye is dominant -> direct output


  		




  
  83
  
    
    float eye_pan_r_target = (requested_gaze_state.pan + requested_gaze_state.vergence/2.0)


  		




  
  84
  
    
            - (neck_pan_now);


  		




  78
  85
  
    

  		




  79
  
  
    
    //left eye is non dominant -> filtered output: TODO: activate low pass filtered output


  		




  80
  
  
    
    //FIXME: USE LOWPASS FILTER HERE --> output_angle[angle_names::EYE_PAN_L] + 0.1 * (eye_pan_l_target - output_angle[angle_names::EYE_PAN_L]) etc;


  		




  81
  
  
    
    float eye_pan_l_target = (requested_gaze_state.pan - requested_gaze_state.vergence/2.0) - (neck_pan_now);


  		




  
  86
  
    
    // left eye is non dominant -> filtered output: TODO: activate low pass filtered output


  		




  
  87
  
    
    // FIXME: USE LOWPASS FILTER HERE --> output_angle[angle_names::EYE_PAN_L]


  		




  
  88
  
    
    //        + 0.1 * (eye_pan_l_target - output_angle[angle_names::EYE_PAN_L]) etc;


  		




  
  89
  
    
    float eye_pan_l_target = (requested_gaze_state.pan - requested_gaze_state.vergence/2.0)


  		




  
  90
  
    
            - (neck_pan_now);


  		




  82
  91
  
    

  		




  83
  
  
    
    //tilt:


  		




  
  92
  
    
    // tilt


  		




  84
  93
  
    
    float eye_tilt_target = requested_gaze_state.tilt - (neck_tilt_now);


  		




  85
  94
  
    

  		




  86
  
  
    
    if (0){


  		




  
  95
  
    
    if (0) {


  		




  87
  96
  
    
        eye_pan_r_target = 0.0;


  		




  88
  97
  
    
        eye_pan_l_target = 0.0;


  		




  89
  98
  
    
        eye_tilt_target  = 0.0;


  		




  90
  99
  
    
    }


  		




  91
  100
  
    

  		




  92
  
  
    
    //check and take care of limits


  		




  
  101
  
    
    // check and take care of limits


  		




  93
  102
  
    
    eye_pan_l_target = limit_target(JointInterface::ID_EYES_LEFT_LR, eye_pan_l_target);


  		




  94
  103
  
    
    eye_pan_r_target = limit_target(JointInterface::ID_EYES_RIGHT_LR, eye_pan_r_target);


  		




  95
  104
  
    
    eye_tilt_target = limit_target(JointInterface::ID_EYES_BOTH_UD, eye_tilt_target);


  		




  96
  105
  
    

  		




  97
  
  
    
    //fetch current dataset


  		




  
  106
  
    
    // fetch current dataset


  		




  98
  107
  
    
    float eye_pan_l_now, eye_pan_r_now, eye_tilt_now;


  		




  99
  108
  
    
    float eye_pan_l_speed, eye_pan_r_speed, eye_tilt_speed;


  		




  100
  109
  
    

  		




  ......




  113
  122
  
    
                &eye_tilt_now,


  		




  114
  123
  
    
                &eye_tilt_speed);


  		




  115
  124
  
    

  		




  116
  
  
    
    //pass paramaters to reflexxes api:


  		




  
  125
  
    
    // pass paramaters to reflexxes api


  		




  117
  126
  
    
    setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now, eye_pan_l_speed, eye_pan_l_ts);


  		




  118
  127
  
    
    setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now, eye_pan_r_speed, eye_pan_r_ts);


  		




  119
  128
  
    
    setup_eyemotion(2, eye_tilt_target, eye_tilt_now, eye_tilt_speed,    eye_tilt_ts);


  		




  120
  129
  
    

  		




  121
  130
  
    
    // cout << "EYE MOTION 2 " << eye_tilt_target << " now=" << eye_tilt_now << "\n";


  		




  122
  131
  
    

  		




  123
  
  
    
    //call reflexxes to handle profile calculation:


  		




  
  132
  
    
    // call reflexxes to handle profile calculation


  		




  124
  133
  
    
    reflexxes_calculate_profile();


  		




  125
  134
  
    

  		




  126
  
  
    
    //tell the joint about the new values:


  		




  
  135
  
    
    // tell the joint about the new values


  		




  127
  136
  
    
    joint_interface->set_target(JointInterface::ID_EYES_LEFT_LR,


  		




  128
  137
  
    
                                reflexxes_position_output->NewPositionVector->VecData[0],


  		




  129
  138
  
    
                                reflexxes_position_output->NewVelocityVector->VecData[0]);


  		




  ......




  137
  146
  
    

  		




  138
  147
  
    

  		




  139
  148
  
    
//! pubish the calculated targets to the joint subsystem


  		




  140
  
  
    
void EyeMotionGenerator::publish_targets(){


  		




  141
  
  
    
    //publish values if there is an active gaze input within the last timerange


  		




  142
  
  
    
    if (gaze_target_input_active()){


  		




  
  149
  
    
void EyeMotionGenerator::publish_targets() {


  		




  
  150
  
    
    // publish values if there is an active gaze input within the last timerange


  		




  
  151
  
    
    if (gaze_target_input_active()) {


  		




  143
  152
  
    
        joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LR);


  		




  144
  153
  
    
        joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_LR);


  		




  145
  154
  
    
        joint_interface->publish_target(JointInterface::ID_EYES_BOTH_UD);


  		












Also available in:  Unified diff