Revision 730467d3 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 current_position, float current_speed){
52 
void EyeMotionGenerator::setup_eyemotion(int dof, float target,
53 
                                         float current_position,
54 
                                         float current_speed,
55 
                                         humotion::Timestamp timestamp){
53 56 
    //get distance to target:
54 57 
    float distance_abs = fabs(target - current_position);
55 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)
......
60 63 
    float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4);
61 64 

  		




  62
  65
  
    
    //feed reflexxes api with data


  		




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


  		




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


  		




  64
  67
  
    
}


  		




  65
  68
  
    

  		




  66
  69
  
    
//! calculate joint targets


  		




  ......




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


  		




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


  		




  93
  96
  
    

  		




  94
  
  
    
    //fetch current angles:


  		




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


  		




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


  		




  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);


  		




  
  97
  
    
    //fetch current dataset


  		




  
  98
  
    
    float eye_pan_l_now, eye_pan_r_now, eye_tilt_now;


  		




  
  99
  
    
    float eye_pan_l_speed, eye_pan_r_speed, eye_tilt_speed;


  		




  
  100
  
    

  		




  
  101
  
    
    humotion::Timestamp eye_pan_l_ts = get_timestamped_state(


  		




  
  102
  
    
                JointInterface::ID_EYES_LEFT_LR,


  		




  
  103
  
    
                &eye_pan_l_now,


  		




  
  104
  
    
                &eye_pan_l_speed);


  		




  
  105
  
    

  		




  
  106
  
    
    humotion::Timestamp eye_pan_r_ts = get_timestamped_state(


  		




  
  107
  
    
                JointInterface::ID_EYES_RIGHT_LR,


  		




  
  108
  
    
                &eye_pan_r_now,


  		




  
  109
  
    
                &eye_pan_r_speed);


  		




  
  110
  
    

  		




  
  111
  
    
    humotion::Timestamp eye_tilt_ts = get_timestamped_state(


  		




  
  112
  
    
                JointInterface::ID_EYES_BOTH_UD,


  		




  
  113
  
    
                &eye_tilt_now,


  		




  
  114
  
    
                &eye_tilt_speed);


  		




  102
  115
  
    

  		




  103
  116
  
    
    //pass paramaters to reflexxes api:


  		




  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);


  		




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


  		




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


  		




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


  		




  107
  120
  
    

  		




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


  		




  109
  122
  
    

  		












Also available in:  Unified diff