Revision 21444915 src/server/reflexxes_motion_generator.cpp

View differences:

src/server/reflexxes_motion_generator.cpp
35 35 
ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, int dof, float t) : MotionGenerator(j){
36 36 
    dof_count = dof;
37 37 

  		




  38
  
  
    
    //create Reflexxes API for 3 DOF


  		




  
  38
  
    
    //create Reflexxes API for <dof> DOF actuator


  		




  39
  39
  
    
    reflexxes_api = new ReflexxesAPI(dof, t);


  		




  40
  40
  
    
    reflexxes_position_input  = new RMLPositionInputParameters(dof);


  		




  41
  41
  
    
    reflexxes_position_output = new RMLPositionOutputParameters(dof);


  		




  ......




  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 max_speed, float max_accel){


  		




  
  57
  
    
void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float position, float max_speed, float max_accel){


  		




  
  58
  
    
    assert(dof < dof_count);


  		




  
  59
  
    

  		




  58
  60
  
    
    //set up reflexxes:


  		




  59
  61
  
    
    reflexxes_position_input->TargetPositionVector->VecData[dof]  = target;


  		




  60
  62
  
    
    reflexxes_position_input->SelectionVector->VecData[dof]       = true;


  		




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


  		




  62
  
  
    
    reflexxes_position_input->MaxAccelerationVector->VecData[dof] = 0.0001 + max_accel;


  		




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


  		




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


  		




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


  		




  
  67
  
    

  		




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


  		




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


  		




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


  		




  
  71
  
    
    }


  		




  
  72
  
    

  		




  64
  73
  
    
}


  		




  65
  74
  
    

  		




  66
  75
  
    
//! calculate motion profile


  		




  ......




  78
  87
  
    

  		




  79
  88
  
    
    //feed back values:


  		




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


  		




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


  		




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


  		




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


  		




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


  		




  84
  93
  
    
    }


  		












Also available in:  Unified diff