Revision 6d13138a src/server/neck_motion_generator.cpp

View differences:

src/server/neck_motion_generator.cpp
32 32 
#include "humotion/server/server.h"
33 33 

  		




  34
  34
  
    
using humotion::server::NeckMotionGenerator;


  		




  35
  
  
    

  		




  36
  
  
    
const float NeckMotionGenerator::CONST_GUITTON87_A = 4.39/2.0;


  		




  37
  
  
    
const float NeckMotionGenerator::CONST_GUITTON87_B = 106.0/2.0;


  		




  38
  
  
    

  		




  39
  
  
    
// healthy adult human: 12-15 breaths/min (see "Ganong's review of medical physiology")


  		




  40
  
  
    
// total: 60/12-15 = 3-5s


  		




  41
  
  
    
// inhale 1.5-2s


  		




  42
  
  
    
// exhale 1.5-2s


  		




  43
  
  
    
// pause      2s


  		




  44
  
  
    
const float NeckMotionGenerator::CONST_BREATH_PERIOD = 1500.0+1500.0+1500.0;  // given in ms


  		




  45
  
  
    
const float NeckMotionGenerator::CONST_BREATH_AMPLITUDE = 1.0;  // degrees


  		




  46
  
  
    

  		




  
  35
  
    
using humotion::server::Config;


  		




  47
  36
  
    

  		




  48
  37
  
    
//! constructor


  		




  49
  
  
    
NeckMotionGenerator::NeckMotionGenerator(JointInterface *j) :


  		




  50
  
  
    
    GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE) {


  		




  
  38
  
    
NeckMotionGenerator::NeckMotionGenerator(JointInterface *j, Config *cfg) :


  		




  
  39
  
    
    GazeMotionGenerator(j, cfg, 3, 1.0/Server::MOTION_UPDATERATE) {


  		




  51
  40
  
    
    breath_time_ = 0.0;


  		




  52
  41
  
    
}


  		




  53
  42
  
    

  		




  ......




  62
  51
  
    
    // we want to have a constant acceleration


  		




  63
  52
  
    
    // -> triangular wave as speeds -> (x<0.5)? 2*x*x:  1- 2*(1-x)**2 = 4x - 2x**2 - 1


  		




  64
  53
  
    
    float breath_offset = 0.0;


  		




  
  54
  
    

  		




  65
  55
  
    
    // 0...1 -> move up, 1..2 -> return, 2..3 -> still


  		




  66
  
  
    
    float breath_time_normalized = (breath_time_ * 3)/CONST_BREATH_PERIOD;


  		




  
  56
  
    
    float breath_time_normalized = (breath_time_ * 3)/config->breath_period;


  		




  67
  57
  
    

  		




  68
  58
  
    
    if (breath_time_normalized <= 0.5) {


  		




  69
  59
  
    
        // accelerated motion


  		




  70
  
  
    
        breath_offset = CONST_BREATH_AMPLITUDE * (2.0 * pow(breath_time_normalized, 2));


  		




  
  60
  
    
        breath_offset = config->breath_amplitude * (2.0 * pow(breath_time_normalized, 2));


  		




  71
  61
  
    
    } else if (breath_time_normalized <= 1.0) {


  		




  72
  62
  
    
        // deaccelerate


  		




  73
  
  
    
        breath_offset = CONST_BREATH_AMPLITUDE * (1.0 - 2.0 * pow(1.0 - breath_time_normalized, 2));


  		




  
  63
  
    
        breath_offset = config->breath_amplitude * (1.0 - 2.0


  		




  
  64
  
    
                                                    * pow(1.0 - breath_time_normalized, 2));


  		




  74
  65
  
    
    } else if (breath_time_normalized <= 1.5) {


  		




  75
  66
  
    
        // accelerate again


  		




  76
  
  
    
        breath_offset = CONST_BREATH_AMPLITUDE * (1.0 - (2.0 * pow(breath_time_normalized-1, 2)));


  		




  
  67
  
    
        breath_offset = config->breath_amplitude * (1.0 - (2.0 * pow(breath_time_normalized-1, 2)));


  		




  77
  68
  
    
    } else if (breath_time_normalized <= 2.0) {


  		




  78
  
  
    
        breath_offset = CONST_BREATH_AMPLITUDE * (2.0 * pow(2.0 - breath_time_normalized, 2));


  		




  
  69
  
    
        breath_offset = config->breath_amplitude * (2.0 * pow(2.0 - breath_time_normalized, 2));


  		




  79
  70
  
    
    } else if (breath_time_normalized <= 3.0) {


  		




  80
  71
  
    
        // pause for some time


  		




  81
  72
  
    
        breath_offset = 0;


  		




  ......




  83
  74
  
    

  		




  84
  75
  
    
    // fetch next time


  		




  85
  76
  
    
    breath_time_ += 1000.0/Server::MOTION_UPDATERATE;


  		




  86
  
  
    
    if (breath_time_ >= CONST_BREATH_PERIOD) {


  		




  87
  
  
    
        breath_time_ -= CONST_BREATH_PERIOD;


  		




  
  77
  
    

  		




  
  78
  
    
    if (breath_time_ >= config->breath_period) {


  		




  
  79
  
    
        breath_time_ -= config->breath_period;


  		




  88
  80
  
    
    }


  		




  89
  81
  
    

  		




  90
  82
  
    
    return breath_offset;


  		




  ......




  205
  197
  
    
    // get distance to target


  		




  206
  198
  
    
    float distance_abs = fabs(target - current_position);


  		




  207
  199
  
    

  		




  208
  
  
    
    // get max speed: according to [guitton87] there is a relation


  		




  
  200
  
    
    // get max speed: according to the equation Hmax from [guitton87] there is a linear relation


  		




  209
  201
  
    
    // between distance_abs and v_max_head:


  		




  210
  202
  
    
    // v_max = 4.39 * d_total + 106.0 (in degrees)


  		




  211
  
  
    
    float max_speed = (CONST_GUITTON87_A * distance_abs + CONST_GUITTON87_B);


  		




  
  203
  
    
    float max_velocity = 4.39 * distance_abs + 106.0;


  		




  
  204
  
    

  		




  
  205
  
    
    // scale and limit max speed:


  		




  
  206
  
    
    max_velocity = max_velocity * config->scale_velocity_neck;


  		




  
  207
  
    
    max_velocity = fmin(max_velocity, config->limit_velocity_neck);


  		




  212
  208
  
    

  		




  213
  209
  
    
    // max accel: assuming linear acceleration we have:


  		




  214
  210
  
    
    /* v ^  _


  		




  ......




  225
  221
  
    
    // and therefore


  		




  226
  222
  
    
    // a = v_max^2 / d_total


  		




  227
  223
  
    
    float max_accel = 0.0;


  		




  
  224
  
    

  		




  228
  225
  
    
    if (distance_abs > 0.0) {


  		




  229
  
  
    
        max_accel = pow(max_speed, 2) / distance_abs;


  		




  
  226
  
    
        max_accel = pow(max_velocity, 2) / distance_abs;


  		




  230
  227
  
    
    }


  		




  231
  228
  
    

  		




  232
  
  
    
    // smoother motion


  		




  233
  
  
    
    max_accel = max_accel * 0.7;


  		




  
  229
  
    
    // scale and limit acceleration


  		




  
  230
  
    
    max_accel = max_accel * config->scale_acceleration_neck;


  		




  
  231
  
    
    max_accel = fmin(max_accel, config->limit_acceleration_neck);


  		




  234
  232
  
    

  		




  235
  
  
    
    // limit maximum acceleration to reduce noise FIXME!


  		




  236
  
  
    
    if (max_accel > 1000) {


  		




  237
  
  
    
        max_accel = 1000;


  		




  238
  
  
    
    }


  		




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


  		




  240
  234
  
    

  		




  241
  235
  
    
    // feed reflexxes api with data


  		




  242
  236
  
    
    reflexxes_set_input(dof, target, current_position, current_velocity,


  		




  243
  
  
    
                        timestamp, max_speed, max_accel);


  		




  
  237
  
    
                        timestamp, max_velocity, max_accel);


  		




  244
  238
  
    
}


  		












Also available in:  Unified diff