Revision 0d0f5ca1 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 position, float max_speed, float max_accel){
57 
void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float current_position, float current_speed, float max_speed, float max_accel){
58 58 
    assert(dof < dof_count);
59 59 

  		




  60
  60
  
    
    //set up reflexxes:


  		




  ......




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


  		




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


  		




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


  		




  66
  
  
    
    reflexxes_position_input->CurrentPositionVector->VecData[dof]  = position;


  		




  
  66
  
    
    // feed back current pos & velocity


  		




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


  		




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


  		




  67
  69
  
    

  		




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


  		




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


  		




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


  		




  71
  73
  
    
    }


  		




  72
  
  
    

  		




  73
  74
  
    
}


  		




  74
  75
  
    

  		




  
  76
  
    

  		




  75
  77
  
    
//! calculate motion profile


  		




  76
  78
  
    

  		




  77
  79
  
    
void ReflexxesMotionGenerator::reflexxes_calculate_profile(){


  		




  ......




  88
  90
  
    
    //feed back values:


  		




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


  		




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


  		




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


  		




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


  		




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


  		




  
  94
  
    
        //reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i];


  		




  93
  95
  
    
    }


  		




  94
  96
  
    
}


  		












Also available in:  Unified diff