Revision 730467d3 src/server/reflexxes_motion_generator.cpp

View differences:

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