walker.cc

该文件是包含了机器人足球比赛中的整个系统的代码

        bestSet.time = currentSet.time;
        
        for (int i=0; i<10; i++) {
          bestFrontLocus[i][0] = currFrontLocus[i][0];
          bestFrontLocus[i][1] = currFrontLocus[i][1];
          bestBackLocus[i][0] = currBackLocus[i][0];
          bestBackLocus[i][1] = currBackLocus[i][1];
        }    
      }

      WriteConfigFile();

      lastSet = currentSet;
      
      if (currentSet.time <= bestSet.time && currentSet.time > 150) ChangeParameters(true);
      else ChangeParameters(false);
    } else {currentSet.time = 0;}
  } 
  //if (!paramLearning && !locusLearning) currentSet.time=0;
  runCount ++;
} 

void Walker::ChangeParameters(bool improved) {
  if (learnType == HILLCLIMBING) HillClimbing(improved);
  else if (learnType == HILLCLIMBING2) HillClimbing2(improved);
}

void Walker::WriteConfigFile() {
  FILE* cfFile = fopen("/ms/open-r/mw/data/walker.cfg","wb");
	
  if (paramLearning) fprintf(cfFile,"paramLearning = true\n");
  else fprintf(cfFile,"paramLearning = false\n");
  if (locusFrontLearning) fprintf(cfFile,"locusFrontLearning = true\n");
  else fprintf(cfFile,"locusFrontLearning = false\n\n");
  if (locusBackLearning) fprintf(cfFile,"locusBackLearning = true\n\n");
  else fprintf(cfFile,"locusBackLearning = false\n\n");

  fprintf(cfFile,"alpha = %f\n",maxAlpha);
  if (decreaseAlpha) fprintf(cfFile,"decreaseAlpha = true\n");
  else fprintf(cfFile,"decreaseAlpha = false\n");
  fprintf(cfFile,"decreaseAlphaRate = %f\n\n",decreaseAlphaRate);

  fprintf(cfFile,"multiplier = %f\n", multiplier);
  fprintf(cfFile,"maxLocusChange = %f\n\n", maxLocusChange);

  fprintf(cfFile,"CurrentStepFreq = %f\n", currentSet.data[0]);
  fprintf(cfFile,"CurrentMaxForward = %f\n", currentSet.data[1]);
  fprintf(cfFile,"CurrentMaxBack = %f\n", currentSet.data[2]);
  fprintf(cfFile,"CurrentFSH = %f\n", currentSet.data[3]);
  fprintf(cfFile,"CurrentFFO = %f\n", currentSet.data[4]);
  fprintf(cfFile,"CurrentFSO = %f\n", currentSet.data[5]);
  fprintf(cfFile,"CurrentFH = %f\n", currentSet.data[6]);
  fprintf(cfFile,"CurrentBSH = %f\n", currentSet.data[7]);
  fprintf(cfFile,"CurrentBFO = %f\n", currentSet.data[8]);
  fprintf(cfFile,"CurrentBSO = %f\n", currentSet.data[9]);
  fprintf(cfFile,"CurrentBH = %f\n", currentSet.data[10]);
  fprintf(cfFile,"CurrentTime = %d\n\n", currentSet.time);
  for (int i=0; i<10; i++) {
    fprintf(cfFile,"CurrentFrontX%d = %f\n", i+1,currFrontLocus[i][0]);
    fprintf(cfFile,"CurrentFrontY%d = %f\n", i+1,currFrontLocus[i][1]);
  }
  for (int i=0; i<10; i++) {
    fprintf(cfFile,"CurrentBackX%d = %f\n", i+1,currBackLocus[i][0]);
    fprintf(cfFile,"CurrentBackY%d = %f\n", i+1,currBackLocus[i][1]);
  }
  fprintf(cfFile,"\nBestStepFreq = %f\n", bestSet.data[0]);
  fprintf(cfFile,"BestMaxForward = %f\n", bestSet.data[1]);
  fprintf(cfFile,"BestMaxBack = %f\n", bestSet.data[2]);
  fprintf(cfFile,"BestFSH = %f\n", bestSet.data[3]);
  fprintf(cfFile,"BestFFO = %f\n", bestSet.data[4]);
  fprintf(cfFile,"BestFSO = %f\n", bestSet.data[5]);
  fprintf(cfFile,"BestFH = %f\n", bestSet.data[6]);
  fprintf(cfFile,"BestBSH = %f\n", bestSet.data[7]);
  fprintf(cfFile,"BestBFO = %f\n", bestSet.data[8]);
  fprintf(cfFile,"BestBSO = %f\n", bestSet.data[9]);
  fprintf(cfFile,"BestBH = %f\n", bestSet.data[10]);
  fprintf(cfFile,"BestTime = %d\n\n", bestSet.time);
  for (int i=0; i<10; i++) {
    fprintf(cfFile,"BestFrontX%d = %f\n", i+1,bestFrontLocus[i][0]);
    fprintf(cfFile,"BestFrontY%d = %f\n", i+1,bestFrontLocus[i][1]);
  }
  for (int i=0; i<10; i++) {
    fprintf(cfFile,"BestBackX%d = %f\n", i+1,bestBackLocus[i][0]);
    fprintf(cfFile,"BestBackY%d = %f\n", i+1,bestBackLocus[i][1]);
  }
  fclose(cfFile);

  // Save into files
  FILE* wmFile = fopen("/ms/open-r/mw/data/walker.waw","a+");
  fprintf(wmFile,"%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", runCount, currentSet.time, currentSet.data[0], currentSet.data[1], currentSet.data[2], currentSet.data[3], currentSet.data[4], currentSet.data[5], currentSet.data[6], currentSet.data[7], currentSet.data[8], currentSet.data[9], currentSet.data[10]);
  for (int i=0; i<10; i++) {
    fprintf(wmFile,",%f,%f", currFrontLocus[i][0],currFrontLocus[i][1]);
  }
  for (int i=0; i<10; i++) {
    fprintf(wmFile,",%f,%f", currBackLocus[i][0],currBackLocus[i][1]);
  } 
  fprintf(wmFile,"\n");
  fclose(wmFile);
}

// This is thes Hill Climbing with a simple inline search
// If and improvement occured then do the same change again
// else select a random change
void Walker::HillClimbing(bool improved) {
  srand48(frame_);
  double num = 0.0;
  if (improved) {
    //Make the same changes again 
    // Double check that front legs don't go less then 3
    if (currentSet.data[fsoIndex]+change.data[fsoIndex] <  3) change.data[fsoIndex]=0;
  } else {
    if (paramLearning) {
      for (int i=0; i<dataSize; i++) {
        bool cont = true;
        while (cont) { // Allows us to limit some of the parameters
          cont = false;
          //if (drand48() >= 0.5) num = 0.0;
          // else 49846599
          num = CauchyDistribution();
          if (i==stepFreqIndex) {
            num=num*0.2;
            while (fabs(num) > 0.4*multiplier) num = 0.2 * CauchyDistribution(); 
          }
          else if (i==maxForwardIndex || i==maxBackIndex) {
            num=num*10.0;
            while (fabs(num) > 20.0*multiplier) num = 10.0 * CauchyDistribution(); 
          } else if (i==fsoIndex) { // limts front side offset to a value > 2 (makes chasing better)
            num=num*5.0;
            while (fabs(num) > 10.0*multiplier /*|| currentSet.data[i]+num < 2*/) num = 5.0 * CauchyDistribution(); 
          } else {
            num=num*5.0;
            while (fabs(num) > 10.0*multiplier) num = 5.0 * CauchyDistribution(); 
          }
        }
        change.data[i] = num; // - change.data[i];
      }
    }
    // Modify Locus
    if (locusFrontLearning) { 
      for (int i=0; i<9; i++) {
          double multiplierL = 1.0;
          if (drand48() >= 0.5) num = 0.0;
          else num = CauchyDistribution()*multiplierL;
          while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
          currFrontLocus[i][0] = bestFrontLocus[i][0]+num;
          if (drand48() >= 0.5) num = 0.0;
          else num = CauchyDistribution()*multiplierL;
          while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
          currFrontLocus[i][1] = bestFrontLocus[i][1]+num;
      }
      currFrontLocus[9][0] = currFrontLocus[0][0];
      currFrontLocus[9][1] = currFrontLocus[0][1];
    }
    if (locusBackLearning) { 
      for (int i=0; i<9; i++) {
          double multiplierL = 1.0;
          if (drand48() >= 0.5) num = 0.0;
          else num = CauchyDistribution()*multiplierL;
          while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
          currBackLocus[i][0] = bestBackLocus[i][0]+num;
          if (drand48() >= 0.5) num = 0.0;
          else num = CauchyDistribution()*multiplierL;
          while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
          currBackLocus[i][1] = bestBackLocus[i][1]+num;
      }
      currBackLocus[9][0] = currBackLocus[0][0];
      currBackLocus[9][1] = currBackLocus[0][1];
    }
  }

  // Update Data Sets
  for (int i=0; i<dataSize; i++) {
    currentSet.data[i]=bestSet.data[i]+change.data[i];   
    cout << currentSet.data[i] << ",";
  }  
  origFrontLocusPoints_ = currFrontLocus; // set the origFrontLocusPoints_ global to point to the currFrontlocus
  origBackLocusPoints_ = currBackLocus; // set the origBackLocusPoints_ global to point to the currBacklocus
   
 cout << endl;
 currentSet.time = 0;
}


// This Hill Climber has a slightly more complicated search
// If an improvement occurs then (alpha)% of the last step is mixed with (1-alpha) of a new random step.
// Alpha is then reduced after each slower run
void Walker::HillClimbing2(bool improved) {
  srand48(frame_);
  if (improved) alpha = maxAlpha;
  double num = 0.0;
  if (paramLearning) {
    for (int i=0; i<dataSize; i++) {
      bool cont = true;
      while (cont) { // Allows us to limit some of the parameters
        cont = false;
        if (drand48() >= 0.5) num = 0.0;
        else num = CauchyDistribution();
        if (i==stepFreqIndex) {
          num=num*0.2;
          while (fabs(num) > 0.4*multiplier) num = 0.2 * CauchyDistribution(); 
        }
        else if (i==maxForwardIndex || i==maxBackIndex) {
          num=num*10.0;
          while (fabs(num) > 20.0*multiplier) num = 10.0 * CauchyDistribution(); 
        } else if (i==fsoIndex) { // limts front side offset to a value > 2 (makes chasing better)
          num=num*5.0;
          while (fabs(num) > 10.0*multiplier || currentSet.data[i]+num < 2) num = 5.0 * CauchyDistribution(); 
        } else {
          num=num*5.0;
          while (fabs(num) > 10.0*multiplier) num = 5.0 * CauchyDistribution(); 
        }
      }
      change.data[i] = (1.0-alpha)*num+alpha*change.data[i];
    }
  }
  // Modify Locus
  if (locusFrontLearning) { 
    for (int i=0; i<9; i++) {
        double multiplierL = 1.0;
        double changeX=currFrontLocus[i][0]-bestFrontLocus[i][0];
        double changeY=currFrontLocus[i][1]-bestFrontLocus[i][1];
        if (drand48() >= 0.5) num = 0.0;
        else num = CauchyDistribution()*multiplierL;
        while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
        currFrontLocus[i][0] = bestFrontLocus[i][0]+((1.0-alpha)*num+alpha*changeX);
        if (drand48() >= 0.5) num = 0.0;
        else num = CauchyDistribution()*multiplierL;
        while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
        currFrontLocus[i][1] = bestFrontLocus[i][1]+((1.0-alpha)*num+alpha*changeY);
    }
    currFrontLocus[9][0] = currFrontLocus[0][0];
    currFrontLocus[9][1] = currFrontLocus[0][1];
  }
  if (locusBackLearning) { 
    for (int i=0; i<9; i++) {
        double multiplierL = 1.0;
        double changeX=currBackLocus[i][0]-bestBackLocus[i][0];
        double changeY=currBackLocus[i][1]-bestBackLocus[i][1];
        if (drand48() >= 0.5) num = 0.0;
        else num = CauchyDistribution()*multiplierL;
        while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
        currBackLocus[i][0] = bestBackLocus[i][0]+((1.0-alpha)*num+alpha*changeX);
        if (drand48() >= 0.5) num = 0.0;
        else num = CauchyDistribution()*multiplierL;
        while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
        currBackLocus[i][1] = bestBackLocus[i][1]+((1.0-alpha)*num+alpha*changeY);
    }
    currBackLocus[9][0] = currBackLocus[0][0];
    currBackLocus[9][1] = currBackLocus[0][1];
  }

  // Update Data Sets
  cout << alpha << ",";
  for (int i=0; i<dataSize; i++) {
    currentSet.data[i]=bestSet.data[i]+change.data[i];   
    cout << currentSet.data[i] << ",";
  }
  cout << endl;

  // Decrease alpha 
  if (decreaseAlpha) alpha=alpha*decreaseAlphaRate;
  
  origFrontLocusPoints_ = currFrontLocus; // set the origFrontLocusPoints_ global to point to the currFrontlocus
  origBackLocusPoints_ = currBackLocus; // set the origBackLocusPoints_ global to point to the currBacklocus
   
  currentSet.time = 0;
}

double Walker::GaussianDistribution(double sigma) {
  double x, y, r2;

  do {
      // Choose x,y in uniform square (-1,-1) to (+1,+1)

      x = -1 + 2 * double(rand()/double(RAND_MAX));
      y = -1 + 2 * double(rand()/double(RAND_MAX));

      // See if it is in the unit circle
      r2 = x * x + y * y;
  } while (r2 > 1.0 || r2 == 0);

  // Box-Muller transform 
  return (sigma * y * sqrt (-2.0 * log(r2) / r2));
}

double Walker::CauchyDistribution() {
  return GaussianDistribution(1.0) / GaussianDistribution(1.0);
}