Revision ea068cf1 src/server/eye_motion_generator.cpp

View differences:

src/server/eye_motion_generator.cpp
75 75 
//! calculate joint targets
76 76 
void EyeMotionGenerator::calculate_targets() {
77 77 
    // use the target values for a faster response
78 
    float neck_pan_now  = joint_interface->get_target_position(JointInterface::ID_NECK_PAN);
79 
    float neck_tilt_now = joint_interface->get_target_position(JointInterface::ID_NECK_TILT);
78 
    float neck_pan_now  = joint_interface_->get_target_position(JointInterface::ID_NECK_PAN);
79 
    float neck_tilt_now = joint_interface_->get_target_position(JointInterface::ID_NECK_TILT);
80 80 

  		




  81
  81
  
    
    // calculate target angles for the eyes


  		




  82
  82
  
    
    // right eye is dominant -> direct output


  		




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


  		




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


  		




  84
  84
  
    
            - (neck_pan_now);


  		




  85
  85
  
    

  		




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


  		




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


  		




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


  		




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


  		




  90
  90
  
    
            - (neck_pan_now);


  		




  91
  91
  
    

  		




  92
  92
  
    
    // tilt


  		




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


  		




  
  93
  
    
    float eye_tilt_target = requested_gaze_state_.tilt - (neck_tilt_now);


  		




  94
  94
  
    

  		




  95
  95
  
    
    if (0) {


  		




  96
  96
  
    
        eye_pan_r_target = 0.0;


  		




  ......




  133
  133
  
    
    reflexxes_calculate_profile();


  		




  134
  134
  
    

  		




  135
  135
  
    
    // tell the joint about the new values


  		




  136
  
  
    
    joint_interface->set_target(JointInterface::ID_EYES_LEFT_LR,


  		




  
  136
  
    
    joint_interface_->set_target(JointInterface::ID_EYES_LEFT_LR,


  		




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


  		




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


  		




  139
  
  
    
    joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LR,


  		




  
  139
  
    
    joint_interface_->set_target(JointInterface::ID_EYES_RIGHT_LR,


  		




  140
  140
  
    
                                reflexxes_position_output->NewPositionVector->VecData[1],


  		




  141
  141
  
    
                                reflexxes_position_output->NewVelocityVector->VecData[1]);


  		




  142
  
  
    
    joint_interface->set_target(JointInterface::ID_EYES_BOTH_UD,


  		




  
  142
  
    
    joint_interface_->set_target(JointInterface::ID_EYES_BOTH_UD,


  		




  143
  143
  
    
                                reflexxes_position_output->NewPositionVector->VecData[2],


  		




  144
  144
  
    
                                reflexxes_position_output->NewVelocityVector->VecData[2]);


  		




  145
  145
  
    
}


  		




  ......




  149
  149
  
    
void EyeMotionGenerator::publish_targets() {


  		




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


  		




  151
  151
  
    
    if (gaze_target_input_active()) {


  		




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


  		




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


  		




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


  		




  
  152
  
    
        joint_interface_->publish_target(JointInterface::ID_EYES_LEFT_LR);


  		




  
  153
  
    
        joint_interface_->publish_target(JointInterface::ID_EYES_RIGHT_LR);


  		




  
  154
  
    
        joint_interface_->publish_target(JointInterface::ID_EYES_BOTH_UD);


  		




  155
  155
  
    
    }


  		




  156
  156
  
    
}


  		












Also available in:  Unified diff