Revision 45345055 src/server/reflexxes_motion_generator.cpp

View differences:

src/server/reflexxes_motion_generator.cpp
60 60 
                                                   float max_speed, float max_accel){
61 61 
    assert(dof < dof_count);
62 62 

  		




  63
  
  
    
    //set up reflexxes:


  		




  
  63
  
    
    // set up reflexxes control loop


  		




  64
  64
  
    
    reflexxes_position_input->TargetPositionVector->VecData[dof]  = target;


  		




  65
  65
  
    
    reflexxes_position_input->SelectionVector->VecData[dof]       = true;


  		




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


  		




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


  		




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


  		




  69
  69
  
    

  		




  70
  
  
    
    /*


  		




  71
  
  
    
    // feed back current pos & velocity


  		




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


  		




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


  		




  74
  
  
    
    float time_diff = humotion::Timestamp::now().to_seconds() - timestamp.to_seconds();


  		




  75
  
  
    
    if (time_diff > 0.1) {


  		




  76
  
  
    
        printf("WARNING: timestamp of position data older than 0.1s (measured %fs)!\n",time_diff);


  		




  77
  
  
    
        printf("         this should never happen! clocks not in sync?\n");


  		




  78
  
  
    
    }


  		




  79
  
  
    
    // assuming this time difference is small we will now do a linear


  		




  80
  
  
    
    // approximation of the current position based on these old measurements


  		




  81
  
  
    
    // under the assumption that the speed is "constant" during this short period


  		




  82
  
  
    
    float position_now = current_position + time_diff * current_speed;


  		




  83
  
  
    

  		




  84
  
  
    

  		




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


  		




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


  		




  87
  
  
    
    */


  		




  88
  
  
    

  		




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


  		




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


  		




  91
  72
  
    
            reflexxes_position_input->MaxAccelerationVector->VecData[dof] = 0.0001;


  		




  ......




  106
  87
  
    
        }


  		




  107
  88
  
    
    }


  		




  108
  89
  
    

  		




  109
  
  
    
    //feed back values:


  		




  
  90
  
    
    // feed back values


  		




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


  		




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


  		




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


  		












Also available in:  Unified diff