Revision 58c9f56c src/server/reflexxes_motion_generator.cpp

View differences:

src/server/reflexxes_motion_generator.cpp
70 70 
    // feed back current pos & velocity
71 71 
    // as we have to deal with some latency we will forecast the current
72 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);
73 
    float time_diff = humotion::Timestamp::now().to_seconds() - timestamp.to_seconds();
74 
    if (time_diff > 0.1) {
75 
        printf("WARNING: timestamp of position data older than 0.1s (measured %fs)!\n",time_diff);
76 
        printf("         this should never happen! clocks not in sync?\n");
77 
    }
78 
    // assuming this time difference is small we will now do a linear
79 
    // approximation of the current position based on these old measurements
80 
    // under the assumption that the speed is "constant" during this short period
81 
    float position_now = current_position + time_diff * current_speed;
82 

  		




  
  83
  
    
    printf("HTS: diff = %f ms, pos=%f --> %f\n", time_diff*1000.0,current_position, position_now);


  		




  75
  84
  
    

  		




  76
  85
  
    

  		




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


  		




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


  		




  
  86
  
    
    reflexxes_position_input->CurrentPositionVector->VecData[dof]  = position_now;


  		




  
  87
  
    
    reflexxes_position_input->CurrentVelocityVector->VecData[dof]  = current_speed;


  		




  79
  88
  
    

  		




  80
  89
  
    
    // safety: libreflexxes does not like zero accellerations...


  		




  81
  90
  
    
    if (reflexxes_position_input->MaxAccelerationVector->VecData[dof] == 0.0){


  		




  ......




  99
  108
  
    

  		




  100
  109
  
    
    //feed back values:


  		




  101
  110
  
    
    for(int i=0; i<dof_count; i++){


  		




  102
  
  
    
        reflexxes_position_input->CurrentPositionVector->VecData[i]     = reflexxes_position_output->NewPositionVector->VecData[i];


  		




  103
  
  
    
        reflexxes_position_input->CurrentVelocityVector->VecData[i]     = reflexxes_position_output->NewVelocityVector->VecData[i];


  		




  
  111
  
    
        //reflexxes_position_input->CurrentPositionVector->VecData[i]     = reflexxes_position_output->NewPositionVector->VecData[i];


  		




  
  112
  
    
        //reflexxes_position_input->CurrentVelocityVector->VecData[i]     = reflexxes_position_output->NewVelocityVector->VecData[i];


  		




  104
  113
  
    
        reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i];


  		




  105
  114
  
    
    }


  		




  106
  115
  
    
}


  		












Also available in:  Unified diff