int executeCB(const soft_move_base::SoftMoveBaseGoalConstPtr &goal)
  {
    ROS_INFO("Received new trajectory");
    SM_TRAJ tmpTraj;
    ros::Rate _loop_rate(1/_dt);
    _timeFromStart= 0;
    _tmpTimeFromStart= 0;
    _timer = 0;
    loadNewTraj(goal->traj); 
    double duration = 0.0;
    duration =  _newTraj.getDuration();


    limits.clear();
    for(int i=0; i<6; ++i)
    {
      limit.maxJerk = _newTraj.jmax[i] * 1.1;
      limit.maxAcc =  _newTraj.amax[i] * 1.1;
      limit.maxVel =  _newTraj.vmax[i] * 1.1;
      limits.push_back(limit);
    }

    //double t = 0;
    //FILE* file;
    //file  = fopen("baseExperiment.dat", "w");


   bool cont = true;
   do {
     // end if goal canceled
     if( as_.isPreemptRequested())
		break;

     getNextRobotCond(_timeFromStart + _dt);
     //printCond(_nextRobotCond_w);

     getPoseInRobotFrame(_nextRobotCond_w);
     //printCond(_nextRobotCond_r);

     _currRobotCond_r[0].x = 0;
     _currRobotCond_r[0].v = _lastVel[0];
     _currRobotCond_r[0].a = _lastAcc[0];
     _currRobotCond_r[1].x = 0;
     _currRobotCond_r[1].v = _lastVel[1];
     _currRobotCond_r[1].a = _lastAcc[1];
     _currRobotCond_r[5].x = 0;
     _currRobotCond_r[5].v = _lastVel[5];
     _currRobotCond_r[5].a = _lastAcc[5];
     //printCond(_currRobotCond_r);

     ROS_INFO("TMP TIME:%f", _tmpTimeFromStart);

     if( _timeFromStart == 0.0) 
     {
       //correction
       tmpTraj.clear(); 

       boundsCond(_currRobotCond_r);
       boundsCond(_nextRobotCond_r);
       tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
  
       _tmpTimeFromStart = _dt;
       //printCond( _newRobotCond_r);
     }
     else if(_timer - 0.5 >= 0.0001 
            || _tmpTimeFromStart > tmpTraj.getDuration())
     {
       _timer = 0;

       //correction
       tmpTraj.clear(); 
       tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
  
       _tmpTimeFromStart = _dt;
     }

     tmpTraj.getMotionCond(_tmpTimeFromStart,_newRobotCond_r);
     _lastVel[0]= _newRobotCond_r[0].v;
     _lastVel[1]= _newRobotCond_r[1].v;
     _lastVel[5]= _newRobotCond_r[5].v;
     _lastAcc[0]= _newRobotCond_r[0].a;
     _lastAcc[1]= _newRobotCond_r[1].a;
     _lastAcc[5]= _newRobotCond_r[5].a;

     //printCond(_newRobotCond_r);
    
     publishNewCond(_newRobotCond_r);

     _timeFromStart += _dt;
     _tmpTimeFromStart += _dt;
     _timer += _dt;
     if(_timeFromStart > _newTraj.getDuration())
       _timeFromStart = _newTraj.getDuration();

     _loop_rate.sleep();

     if(_timeFromStart >= _newTraj.getDuration())
     {
       cont = fabs(_newRobotCond_r[0].x) > _tolerance[0]
              || fabs(_newRobotCond_r[1].x) > _tolerance[1]
              || fabs(_newRobotCond_r[5].x) > _tolerance[2];
     }
     
     ros::spinOnce();
     
   } while(cont);

    soft_move_base::SoftMoveBaseResult result;
    result.result = true;
    as_.setSucceeded(result);

    return 0;
  } 
 void getNextRobotCond(double time)
 {
   _newTraj.getMotionCond(time, _nextRobotCond_w);
 }
Exemplo n.º 3
0
int Sm_Approx::approximate(Sm_Curve &curv, double SampTime,  double ErrPosMax, double ErrVelMax, int ExpTime, bool flagExport, std::string fileName, SM_TRAJ &smTraj)
{
  std::string str2;
  FILE *fp_segMotion = NULL;
  _sampling = SampTime;
  _errMax = ErrPosMax;
  _timeStep = ExpTime;
  _flag_haus_actif = 0;
  _fileName = fileName;
  _nbAxis = curv.traj[0].Pos.size() ;
  SM_OUTPUT tempo_motion;
     double tu, ts;


  /* set limits that are only used to compute the error in velocity on overall the trajectory */
  _lim.maxJerk = 6*fabs(ErrVelMax);
  _lim.maxAcc  = 2*fabs(ErrVelMax);
  _lim.maxVel  = fabs(ErrVelMax);
   
  initializeApproxVariables();

  // dans l'attribut _curve il y aura deux courbes, la premiere _curve[0] est la traj ideale
  // la second _curve[1] = (_curve.back()) sera la traj approximee
  // les suites de cubiques de la traj approxime sont stockées dans _result
  // _result.size() est la nombre de segment de cubique (c'est la meme pour tout les axes)

  // SAUVE LA TRAJCTOIRE IDEALE (sous la forme d'un tableau)
  if(flagExport==true) {
    str2.clear();
    str2 += "SmIdealTraj.dat";
    saveTraj(str2, curv.traj);
    SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Ideal Trajectory Already Computed and Saved");
  }

  /* Handle the path */
  _curve.push_back(curv);

  ChronoOn();
  ChronoTimes(&tu, &ts);
  /// CALCUL DE L'APPROXIMATION //
  computeTraj();
  SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Computation Duration : ");
  ChronoPrint("");
  std::cout << std::endl;
  ChronoTimes(&tu, &ts);
  ChronoOff();


  if(flagExport==true) {
    // SAUVE LA TRAJCTOIRE APPROXIMEE (sous la forme d'un tableau)
    saveTraj("SmApproxTraj.dat", (_curve.back()).traj);
    SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Approximated Trajectory Saved");
  }

  SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Error Max Pos " << (_curve.back()).errorMaxVal);

  /* fill result */
  for (unsigned int i = 1; i < _result.size(); i++){
    for (unsigned int j = 0; j < _result.size()-i ; j++){
      if (_result[j].premier_point > _result[j+1].premier_point){
        tempo_motion = _result[j];
        _result[j] = _result[j+1];
        _result[j+1] = tempo_motion;
      }
    }
  }

  _result[0].IC.resize(_result[0].Time.size());

  for(unsigned int i=0; i<_result[0].Time.size(); i++) {
    _result[0].IC[i].x = curv.traj[0].Pos[i];
  }

  if(flagExport==true) {
    //printf("approximate: There are %f axes and %f segments\n", (double)_result[0].Time.size(), (double)_result.size());  
    fp_segMotion = fopen("SmSegMotion.dat", "w");
    if(fp_segMotion==NULL) {
      SM_LOG(SM_LOG_ERROR, " cannot open file to write the trajectory");
      return 1;
    }
    for (unsigned int i = 0; i < _result.size(); i++){
      fprintf(fp_segMotion, "%d %lf ",_result[i].premier_point, (_result[i].premier_point + 1) * _sampling);
      for(unsigned int k=0; k<_result[i].Time.size(); k++) {
	fprintf(fp_segMotion, "%lf %lf ",_result[i].Time[k],_result[i].Jerk[k]);
      }
      fprintf(fp_segMotion, "\n");
    }
    fclose(fp_segMotion);
    //cout << "motion file exported" << endl;
    
    //printf("approximate: There are %f axes and %f segments\n", (double)_result[0].Time.size(), (double)_result.size());
  }

  // importe le resultat _result dans une classe de type SM_TRAJ Attention on recalcule tte les conditions initiales
  // de chaque segment dans cette fonction ainsi que la duree totale de la trajectoire a partir des couples (Ti, Ji)
  smTraj.importFromSM_OUTPUT(36, _sampling, _result);

  //cout << " ... Approximated Trajectory Computed --" << endl;

  if(flagExport==true) {  
    smTraj.save((char*)"SmApproxTraj_Seg.traj");
    SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate Series of cubics Trajectory Saved");
  }
  /* compare end point */
  /* verification du point final atteint par smTraj par rapport a IdealTraj */
  std::vector< SM_COND> cond;
  smTraj.getMotionCond(smTraj.getDuration(), cond);
  double errMax =0;
  for(unsigned int i=0; i<cond.size(); i++) {
    double err = fabs(cond[i].x -  (_curve.back()).traj[ (_curve.back()).traj.size()-1].Pos[i] );
    if( err > 0.001) {
        SM_LOG(SM_LOG_WARNING, "Sm_Approx::approximate: final pose on axis " << i << ", err=" << err);
    }
    if(err > errMax){
      errMax = err;
    }
  }
  if(errMax < 0.001) {
    SM_LOG(SM_LOG_INFO, "Sm_Approx::approximate runs successfully ... Algo Written by Xavier BROQUERE (Feb 2011)");
  }
  //printf("Final Conditions of the Ideal trajectory\n");
  //for(unsigned int i=0; i<cond.size(); i++) {
  //  printf("%f ",(_curve.back()).traj[ (_curve.back()).traj.size()-1].Pos[i] );
  //}
  //printf("\n"); 
  //printf("Final Conditions of the smTraj trajectory\n");
  //for(unsigned int i=0; i<cond.size(); i++) {
  //  printf("%f ",cond[i].x);
  //}
  //printf("\n");

  return 0;
}