Revision 0d0f5ca1 src/server/eye_motion_generator.cpp

View differences:

src/server/eye_motion_generator.cpp
49 49 
//! \param dof id of joint
50 50 
//! \param target angle
51 51 
//! \param current angle
52 
void EyeMotionGenerator::setup_eyemotion(int dof, float target, float now){
52 
void EyeMotionGenerator::setup_eyemotion(int dof, float target, float current_position, float current_speed){
53 53 
    //get distance to target:
54 
    float distance_abs = fabs(target - now);
54 
    float distance_abs = fabs(target - current_position);
55 55 
    //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)
56 56 
    float max_speed = fmin(700.0, 10.0*distance_abs + 200.0);
57 57 
    //max accel: use data from ""Speed and Accuracy of Saccadic Eye Movements: Characteristics of Impulse Variability in the Oculomotor System"
......
60 60 
    float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4);
61 61 

  		




  62
  62
  
    
    //feed reflexxes api with data


  		




  63
  
  
    
    reflexxes_set_input(dof, target, now, max_speed, max_accel);


  		




  
  63
  
    
    reflexxes_set_input(dof, target, current_position, current_speed, max_speed, max_accel);


  		




  64
  64
  
    
}


  		




  65
  65
  
    

  		




  66
  66
  
    
//! calculate joint targets


  		




  ......




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


  		




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


  		




  97
  97
  
    
    float eye_tilt_now  = get_current_position(JointInterface::ID_EYES_BOTH_UD);


  		




  
  98
  
    
    //fetch current velocities


  		




  
  99
  
    
    float eye_pan_l_speed = get_current_speed(JointInterface::ID_EYES_LEFT_LR);


  		




  
  100
  
    
    float eye_pan_r_speed = get_current_speed(JointInterface::ID_EYES_RIGHT_LR);


  		




  
  101
  
    
    float eye_tilt_speed  = get_current_speed(JointInterface::ID_EYES_BOTH_UD);


  		




  98
  102
  
    

  		




  99
  103
  
    
    //pass paramaters to reflexxes api:


  		




  100
  
  
    
    setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now);


  		




  101
  
  
    
    setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now);


  		




  102
  
  
    
    setup_eyemotion(2, eye_tilt_target, eye_tilt_now);


  		




  103
  
  
    
    cout << "EYE MOTION 2 " << eye_tilt_target << " now=" << eye_tilt_now << "\n";


  		




  
  104
  
    
    setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now, eye_pan_l_speed);


  		




  
  105
  
    
    setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now, eye_pan_r_speed);


  		




  
  106
  
    
    setup_eyemotion(2, eye_tilt_target, eye_tilt_now, eye_tilt_speed);


  		




  
  107
  
    

  		




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


  		




  104
  109
  
    

  		




  105
  110
  
    
    //call reflexxes to handle profile calculation:


  		




  106
  111
  
    
    reflexxes_calculate_profile();


  		












Also available in:  Unified diff