Revision 730467d3

View differences:

include/humotion/server/eye_motion_generator.h
43 43 

  		




  44
  44
  
    
    static const float SACCADE_SPEED_THRESHOLD;


  		




  45
  45
  
    
private:


  		




  46
  
  
    
    void setup_eyemotion(int dof, float target, float current_position, float current_speed);


  		




  
  46
  
    
    void setup_eyemotion(int dof, float target, float current_position, float current_speed, Timestamp timestamp);


  		




  47
  47
  
    

  		




  48
  48
  
    
    TimestampedList tsl_gaze_target_pan;


  		




  49
  49
  
    
    TimestampedList tsl_gaze_target_tilt;


  		












  

    
      include/humotion/server/motion_generator.h
    
  





  47
  47
  
    
protected:


  		




  48
  48
  
    
    float get_current_position(int joint_id);


  		




  49
  49
  
    
    float get_current_speed(int joint_id);


  		




  
  50
  
    
    humotion::Timestamp get_timestamped_state(int joint_id, float *position, float *velocity);


  		




  50
  51
  
    
    float limit_target(int joint_id, float val);


  		




  51
  52
  
    

  		




  52
  53
  
    
    JointInterface *joint_interface;


  		












  

    
      include/humotion/server/neck_motion_generator.h
    
  





  37
  37
  
    
    ~NeckMotionGenerator();


  		




  38
  38
  
    
    void calculate_targets();


  		




  39
  39
  
    
    void publish_targets();


  		




  40
  
  
    
    void setup_neckmotion(int dof, float target, float current_position, float current_speed);


  		




  
  40
  
    
    void setup_neckmotion(int dof, float target, float current_position, float current_speed, Timestamp timestamp);


  		




  41
  41
  
    

  		




  42
  42
  
    
private:


  		




  43
  43
  
    
    float get_breath_offset();


  		












  

    
      include/humotion/server/reflexxes_motion_generator.h
    
  





  41
  41
  
    
    ~ReflexxesMotionGenerator();


  		




  42
  42
  
    

  		




  43
  43
  
    
protected:


  		




  44
  
  
    
    void reflexxes_set_input(int dof, float target, float current_position, float current_speed, float max_speed, float max_accel);


  		




  
  44
  
    
    void reflexxes_set_input(int dof, float target, float current_position, float current_speed, Timestamp timestamp, float max_speed, float max_accel);


  		




  45
  45
  
    
    void reflexxes_calculate_profile();


  		




  46
  46
  
    

  		




  47
  47
  
    
    //*****************************************


  		












  

    
      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
  
    

  		












  

    
      src/server/motion_generator.cpp
    
  





  45
  45
  
    
MotionGenerator::~MotionGenerator(){


  		




  46
  46
  
    
}


  		




  47
  47
  
    

  		




  
  48
  
    
//! fetch the latest position and velocity and return the timestamp of that dataset


  		




  
  49
  
    
//! \param joint id


  		




  
  50
  
    
//! \param pointer to position variable


  		




  
  51
  
    
//! \param pointer to veolocity variable


  		




  
  52
  
    
//! \return Timestamp of this dataset


  		




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


  		




  
  54
  
    
                                                           float *position, float *velocity) {


  		




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


  		




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


  		




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


  		




  
  58
  
    
    return stamp;


  		




  
  59
  
    
}


  		




  
  60
  
    

  		




  48
  61
  
    
//! fetch the latest (=current) speed of a joint


  		




  49
  62
  
    
//! \param joint_id


  		




  50
  63
  
    
//! \return float value of joint speed


  		












  

    
      src/server/neck_motion_generator.cpp
    
  





  91
  91
  
    

  		




  92
  92
  
    
//! calculate joint targets


  		




  93
  93
  
    
void NeckMotionGenerator::calculate_targets(){


  		




  94
  
  
    
    //fetch current angles:


  		




  95
  
  
    
    float neck_pan_now  = get_current_position(JointInterface::ID_NECK_PAN);


  		




  96
  
  
    
    float neck_tilt_now = get_current_position(JointInterface::ID_NECK_TILT);


  		




  97
  
  
    
    float neck_roll_now = get_current_position(JointInterface::ID_NECK_ROLL);


  		




  98
  
  
    
    // fetch current velocities


  		




  99
  
  
    
    float neck_pan_speed  = get_current_speed(JointInterface::ID_NECK_PAN);


  		




  100
  
  
    
    float neck_tilt_speed = get_current_speed(JointInterface::ID_NECK_TILT);


  		




  101
  
  
    
    float neck_roll_speed = get_current_speed(JointInterface::ID_NECK_ROLL);


  		




  
  94
  
    
    //fetch current dataset:


  		




  
  95
  
    
    float neck_pan_now, neck_tilt_now, neck_roll_now;


  		




  
  96
  
    
    float neck_pan_speed, neck_tilt_speed, neck_roll_speed;


  		




  
  97
  
    

  		




  
  98
  
    
    humotion::Timestamp neck_pan_ts = get_timestamped_state(JointInterface::ID_NECK_PAN,


  		




  
  99
  
    
                                                            &neck_pan_now,


  		




  
  100
  
    
                                                            &neck_pan_speed);


  		




  
  101
  
    

  		




  
  102
  
    
    humotion::Timestamp neck_tilt_ts = get_timestamped_state(JointInterface::ID_NECK_TILT,


  		




  
  103
  
    
                                                            &neck_tilt_now,


  		




  
  104
  
    
                                                            &neck_tilt_speed);


  		




  
  105
  
    

  		




  
  106
  
    
    humotion::Timestamp neck_roll_ts = get_timestamped_state(JointInterface::ID_NECK_ROLL,


  		




  
  107
  
    
                                                            &neck_roll_now,


  		




  
  108
  
    
                                                            &neck_roll_speed);


  		




  102
  109
  
    

  		




  103
  110
  
    
    //reached target?


  		




  104
  111
  
    
    float goal_diff   = fabs(get_current_gaze().distance_pt_abs(requested_gaze_state));


  		




  ......




  147
  154
  
    
    neck_tilt_target += get_breath_offset();


  		




  148
  155
  
    

  		




  149
  156
  
    
    //pass parameters to reflexxes api:


  		




  150
  
  
    
    setup_neckmotion(0, neck_pan_target,  neck_pan_now, neck_pan_speed);


  		




  151
  
  
    
    setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed);


  		




  152
  
  
    
    setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed);


  		




  
  157
  
    
    setup_neckmotion(0, neck_pan_target,  neck_pan_now,  neck_pan_speed,  neck_pan_ts);


  		




  
  158
  
    
    setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed, neck_tilt_ts);


  		




  
  159
  
    
    setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed, neck_roll_ts);


  		




  153
  160
  
    

  		




  154
  161
  
    
    //call reflexxes to handle profile calculation:


  		




  155
  162
  
    
    reflexxes_calculate_profile();


  		




  ......




  167
  174
  
    
                                         reflexxes_position_output->NewPositionVector->VecData[2],


  		




  168
  175
  
    
                                         reflexxes_position_output->NewVelocityVector->VecData[2]);


  		




  169
  176
  
    

  		




  170
  
  
    
    printf("\n%f %f %f %f %f %f DBG\n",


  		




  
  177
  
    
    printf("\n%f %f %f %f %f DBG\n",


  		




  171
  178
  
    
            neck_pan_now, neck_pan_target,


  		




  172
  179
  
    
            reflexxes_position_output->NewPositionVector->VecData[0],


  		




  173
  180
  
    
            joint_interface->get_ts_speed(JointInterface::ID_NECK_PAN).get_newest_value(),


  		




  ......




  191
  198
  
    
//! \param dof id of joint


  		




  192
  199
  
    
//! \param target angle


  		




  193
  200
  
    
//! \param current angle


  		




  194
  
  
    
void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position, float current_speed){


  		




  
  201
  
    
void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position,


  		




  
  202
  
    
                                           float current_speed, humotion::Timestamp timestamp){


  		




  195
  203
  
    
    //get distance to target:


  		




  196
  204
  
    
    float distance_abs = fabs(target - current_position);


  		




  197
  205
  
    

  		




  ......




  228
  236
  
    
    ///printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);


  		




  229
  237
  
    

  		




  230
  238
  
    
    //feed reflexxes api with data


  		




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


  		




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


  		




  232
  240
  
    
}


  		












  

    
      src/server/reflexxes_motion_generator.cpp
    
  





  54
  54
  
    
//! \param target angle


  		




  55
  55
  
    
//! \param max_speed max reachable speed during accel


  		




  56
  56
  
    
//! \param max_accel max allowable acceleration


  		




  57
  
  
    
void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float current_position, float current_speed, float max_speed, float max_accel){


  		




  
  57
  
    
void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target,


  		




  
  58
  
    
                                                   float current_position, float current_speed,


  		




  
  59
  
    
                                                   humotion::Timestamp timestamp,


  		




  
  60
  
    
                                                   float max_speed, float max_accel){


  		




  58
  61
  
    
    assert(dof < dof_count);


  		




  59
  62
  
    

  		




  60
  63
  
    
    //set up reflexxes:


  		




  ......




  63
  66
  
    
    reflexxes_position_input->MaxVelocityVector->VecData[dof]     = max_speed;


  		




  64
  67
  
    
    reflexxes_position_input->MaxAccelerationVector->VecData[dof] = max_accel;


  		




  65
  68
  
    
    reflexxes_position_input->TargetVelocityVector->VecData[dof]  = 0.0; //target speed is zero (really?)


  		




  
  69
  
    

  		




  66
  70
  
    
    // feed back current pos & velocity


  		




  
  71
  
    
    // as we have to deal with some latency we will forecast the current


  		




  
  72
  
    
    // position using the old speed, position and the latency:


  		




  
  73
  
    
    float diff = humotion::Timestamp::now().to_seconds() - timestamp.to_seconds();


  		




  
  74
  
    
    printf("HTS: diff = %f ms\n", diff*1000.0);


  		




  
  75
  
    

  		




  
  76
  
    

  		




  67
  77
  
    
    //reflexxes_position_input->CurrentPositionVector->VecData[dof]  = current_position;


  		




  68
  78
  
    
    //reflexxes_position_input->CurrentVelocityVector->VecData[dof]  = current_speed;


  		




  69
  79
  
    

  		












Also available in:  Unified diff