Revision ea068cf1 src/server/motion_generator.cpp

View differences:

src/server/motion_generator.cpp
32 32 

  		




  33
  33
  
    
//! constructor


  		




  34
  34
  
    
MotionGenerator::MotionGenerator(JointInterface *j) {


  		




  35
  
  
    
    joint_interface = j;


  		




  36
  
  
    
    last_mouth_target_update = boost::posix_time::ptime(boost::posix_time::min_date_time);


  		




  37
  
  
    
    last_gaze_target_update  = boost::posix_time::ptime(boost::posix_time::min_date_time);


  		




  
  35
  
    
    joint_interface_ = j;


  		




  
  36
  
    
    last_mouth_target_update_ = boost::posix_time::ptime(boost::posix_time::min_date_time);


  		




  
  37
  
    
    last_gaze_target_update_  = boost::posix_time::ptime(boost::posix_time::min_date_time);


  		




  38
  38
  
    
}


  		




  39
  39
  
    

  		




  40
  40
  
    

  		




  ......




  49
  49
  
    
//! \return Timestamp of this dataset


  		




  50
  50
  
    
humotion::Timestamp MotionGenerator::get_timestamped_state(int joint_id,


  		




  51
  51
  
    
                                                           float *position, float *velocity) {


  		




  52
  
  
    
    humotion::Timestamp stamp = joint_interface->get_ts_position(joint_id).get_last_timestamp();


  		




  53
  
  
    
    *position = joint_interface->get_ts_position(joint_id).get_interpolated_value(stamp);


  		




  54
  
  
    
    *velocity = joint_interface->get_ts_speed(joint_id).get_interpolated_value(stamp);


  		




  
  52
  
    
    humotion::Timestamp stamp = joint_interface_->get_ts_position(joint_id).get_last_timestamp();


  		




  
  53
  
    
    *position = joint_interface_->get_ts_position(joint_id).get_interpolated_value(stamp);


  		




  
  54
  
    
    *velocity = joint_interface_->get_ts_speed(joint_id).get_interpolated_value(stamp);


  		




  55
  55
  
    
    return stamp;


  		




  56
  56
  
    
}


  		




  57
  57
  
    

  		




  ......




  59
  59
  
    
//! \param joint_id


  		




  60
  60
  
    
//! \return float value of joint speed


  		




  61
  61
  
    
float MotionGenerator::get_current_speed(int joint_id) {


  		




  62
  
  
    
    return joint_interface->get_ts_speed(joint_id).get_newest_value();


  		




  
  62
  
    
    return joint_interface_->get_ts_speed(joint_id).get_newest_value();


  		




  63
  63
  
    
}


  		




  64
  64
  
    

  		




  65
  65
  
    
//! fetch the latest (=current) position of a joint


  		




  ......




  71
  71
  
    
    Timestamp diff=now-tsl;


  		




  72
  72
  
    
    printf("TIME DIFF %fs %fns\n",diff.sec, diff.nsec);*/


  		




  73
  73
  
    

  		




  74
  
  
    
    return joint_interface->get_ts_position(joint_id).get_newest_value();


  		




  
  74
  
    
    return joint_interface_->get_ts_position(joint_id).get_newest_value();


  		




  75
  75
  
    
}


  		




  76
  76
  
    

  		




  77
  77
  
    
//! update gaze target:


  		




  78
  78
  
    
//! \param GazeState with target values for the overall gaze


  		




  79
  79
  
    
void MotionGenerator::set_gaze_target(GazeState new_gaze_target) {


  		




  80
  80
  
    
    // store value for next iteration


  		




  81
  
  
    
    requested_gaze_state        = new_gaze_target;


  		




  
  81
  
    
    requested_gaze_state_        = new_gaze_target;


  		




  82
  82
  
    

  		




  83
  83
  
    
    // keep track if the gaze targets are comming in regulary


  		




  84
  
  
    
    last_gaze_target_update = boost::get_system_time();


  		




  
  84
  
    
    last_gaze_target_update_ = boost::get_system_time();


  		




  85
  85
  
    
}


  		




  86
  86
  
    

  		




  87
  87
  
    
//! update mouth state:


  		




  88
  88
  
    
//! \param MouthState with target values for the mouth joints


  		




  89
  89
  
    
void MotionGenerator::set_mouth_target(MouthState s) {


  		




  90
  90
  
    
    // store value


  		




  91
  
  
    
    requested_mouth_target = s;


  		




  
  91
  
    
    requested_mouth_target_ = s;


  		




  92
  92
  
    

  		




  93
  93
  
    
    // keep track if the mouth targets are comming in regulary


  		




  94
  
  
    
    last_mouth_target_update = boost::get_system_time();


  		




  
  94
  
    
    last_mouth_target_update_ = boost::get_system_time();


  		




  95
  95
  
    
}


  		




  96
  96
  
    

  		




  97
  97
  
    

  		




  98
  98
  
    
//! was there incoming gaze data the last second?


  		




  99
  99
  
    
//! \return true if there was data incoming in the last second, false otherwise


  		




  100
  100
  
    
bool MotionGenerator::gaze_target_input_active() {


  		




  101
  
  
    
    if (last_gaze_target_update  + boost::posix_time::milliseconds(1000)


  		




  
  101
  
    
    if (last_gaze_target_update_  + boost::posix_time::milliseconds(1000)


  		




  102
  102
  
    
            > boost::get_system_time() ) {


  		




  103
  103
  
    
        // incoming data -> if gaze is disabled, enable it!


  		




  104
  
  
    
        joint_interface->enable_gaze_joints();


  		




  
  104
  
    
        joint_interface_->enable_gaze_joints();


  		




  105
  105
  
    
        return true;


  		




  106
  106
  
    
    }


  		




  107
  107
  
    

  		




  108
  108
  
    
    // else: no incoming data, disable!


  		




  109
  
  
    
    joint_interface->disable_gaze_joints();


  		




  
  109
  
    
    joint_interface_->disable_gaze_joints();


  		




  110
  110
  
    
    return false;


  		




  111
  111
  
    
}


  		




  112
  112
  
    

  		




  113
  113
  
    
//! was there incoming mouth data the last second?


  		




  114
  114
  
    
//! \return true if there was data incoming in the last second, false otherwise


  		




  115
  115
  
    
bool MotionGenerator::mouth_target_input_active() {


  		




  116
  
  
    
    if (last_mouth_target_update + boost::posix_time::milliseconds(1000)


  		




  
  116
  
    
    if (last_mouth_target_update_ + boost::posix_time::milliseconds(1000)


  		




  117
  117
  
    
            > boost::get_system_time() ) {


  		




  118
  118
  
    
        // incoming data -> if mouth is disabled, enable it!


  		




  119
  
  
    
        joint_interface->enable_mouth_joints();


  		




  
  119
  
    
        joint_interface_->enable_mouth_joints();


  		




  120
  120
  
    
        return true;


  		




  121
  121
  
    
    }


  		




  122
  122
  
    

  		




  123
  123
  
    
    // else: no incoming data, disable!


  		




  124
  
  
    
    joint_interface->disable_mouth_joints();


  		




  
  124
  
    
    joint_interface_->disable_mouth_joints();


  		




  125
  125
  
    
    return false;


  		




  126
  126
  
    
}


  		




  127
  127
  
    

  		




  ......




  130
  130
  
    
    assert(joint_id < JointInterface::JOINT_ID_ENUM_SIZE);


  		




  131
  131
  
    

  		




  132
  132
  
    
    // fetch min/max for joint:


  		




  133
  
  
    
    float min = joint_interface->get_joint_min(joint_id);


  		




  134
  
  
    
    float max = joint_interface->get_joint_max(joint_id);


  		




  
  133
  
    
    float min = joint_interface_->get_joint_min(joint_id);


  		




  
  134
  
    
    float max = joint_interface_->get_joint_max(joint_id);


  		




  135
  135
  
    

  		




  136
  136
  
    
    if (max < min) {


  		




  137
  137
  
    
        printf("> ERROR: how can min (%4.2f) be bigger than max (%4.2f)?? EXITING NOW\n", min, max);


  		












Also available in:  Unified diff