ehcm.cc

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

    double numSteps = lcq_.GetNumSteps();
    cout <<  "(" << trials << ")   Walk Distance = " << walkDistance << "     Speed = " << newspeed << "    (Best = " << bestSet.speed << ")    Beacon Distance = " << beaconDistance << " (" << beaconCount << ")   NumSteps = " << numSteps << endl << flush;
      
    prevBeaconDistance = beaconDistance;
    
    prevWalkDistance = walkDistance;
}

void EHCM::StartTimer() {
  startFrame = frame_;
  lcq_.ResetNumSteps();
  ((MovementTricks::ChaseBeacon*)currentTrick)->Set(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]);
}

void EHCM::EndTimer(bool worked) {
  endFrame = frame_;
  if (worked) currentSet.time = currentSet.time + (endFrame - startFrame);
  else currentSet.time = currentSet.time + 600;
  if (currentSet.time < 120) {
    cout << "Ignore last run\n";
    currentSet.time = 600;
  }
  //cout << "(" << runCount << ")  Frames = " << currentSet.time << "  (Best = " << bestSet.time << "),  NumSteps = " << numSteps << endl; //  = " << 195.0/((double(endFrame-startFrame))/25.0) << " cm/s \n";
  
  //CheckImprovement();
} 

void EHCM::CheckImprovement() {
  if (targetGP) { // Only learn after going both directions .. there seems to be a difference
    if (runCount > 0) {
      walkP_.run = trials;
      walkP_.time = currentSet.time;
      walkP_.speed = currentSet.speed;
      for (int j=0; j<dataSize; j++) { 
        walkP_.data[j] = currentSet.data[j];
      }
      for (int i=0; i<10; i++) {
          walkP_.frontXpoints[i] = currentSet.frontLocus[i][0];
          walkP_.frontYpoints[i] = currentSet.frontLocus[i][1];
          walkP_.backXpoints[i] = currentSet.backLocus[i][0];
          walkP_.backYpoints[i] = currentSet.backLocus[i][1];
      }
      walkP_.add = true;

      trials++;
      // Update Variables if faster !
      if (currentSet.speed > bestSet.speed) UpdateVariables(); 

      WriteConfigFile();

      lastSet = currentSet;
      lastChangeSet = changeSet;
      
      // Change the Parameters for the next run
      ChangeParameters();
    } else {currentSet.time = 0;}
  }
  runCount ++;
  targetGP = !targetGP; // change beacon target
  if (targetGP) targetBeacon = vo_greenPinkBeacon_;
  else targetBeacon = vo_pinkGreenBeacon_;

}

void EHCM::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");
  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", currentSet.time);
  fprintf(cfFile,"CurrentSpeed = %f\n\n", currentSet.speed);
  for (int i=0; i<10; i++) {
    fprintf(cfFile,"CurrentFrontX%d = %f\n", i+1,currentSet.frontLocus[i][0]);
    fprintf(cfFile,"CurrentFrontY%d = %f\n", i+1,currentSet.frontLocus[i][1]);
  }
  for (int i=0; i<10; i++) {
    fprintf(cfFile,"CurrentBackX%d = %f\n", i+1,currentSet.backLocus[i][0]);
    fprintf(cfFile,"CurrentBackY%d = %f\n", i+1,currentSet.backLocus[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", bestSet.time);
  fprintf(cfFile,"BestSpeed = %f\n\n", bestSet.speed);

  for (int i=0; i<10; i++) {
    fprintf(cfFile,"BestFrontX%d = %f\n", i+1,bestSet.frontLocus[i][0]);
    fprintf(cfFile,"BestFrontY%d = %f\n", i+1,bestSet.frontLocus[i][1]);
  }
  for (int i=0; i<10; i++) {
    fprintf(cfFile,"BestBackX%d = %f\n", i+1,bestSet.backLocus[i][0]);
    fprintf(cfFile,"BestBackY%d = %f\n", i+1,bestSet.backLocus[i][1]);
  }
  fclose(cfFile);

  // Save into files
  FILE* wmFile = fopen("/ms/open-r/mw/data/EHCM.waw","a+");
  fprintf(wmFile,"%d,%f,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", runCount, currentSet.speed, 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", currentSet.frontLocus[i][0],currentSet.frontLocus[i][1]);
  }
  for (int i=0; i<10; i++) {
    fprintf(wmFile,",%f,%f", currentSet.backLocus[i][0],currentSet.backLocus[i][1]);
  } 
  fprintf(wmFile,"\n");
  fclose(wmFile);
}

void EHCM::UpdateVariables() {
  // Set EHCM variables
  double deltaSpeedM = currentSet.speed - bestSet.speed;  
  cout << "DeltaspeedM = " << deltaSpeedM << " (" << currentSet.speed << "-" << bestSet.speed << ")\n";
  alpha = (double) tanh((double)deltaSpeedC/(double)deltaSpeedM);
  cout << "alpha = " << alpha << endl;

  if (deltaSpeedC==1.0 && alpha < 0.1) alpha = 0.5;
  if (alpha >= 0.90) alpha = 0.90;

  trials = 0; // Added

  // Update internal best variables 
  deltaSpeedC = deltaSpeedM;

  prevBestSet = bestSet;
  bestSet = currentSet;
  changeBestSet = changeSet;
}

void EHCM::ChangeParameters() {
  srand48(frame_);

  if (trials > LIMIT) {
    alpha = 1.0;
    trials = 0;
    bestSet.speed = bestSet.speed * 0.9;
  } 
   
  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;
        num = CauchyDistribution();
        multiplier = GaussianDistribution(1.0)*(SIG+SIG*pow((double)trials/(double)LIMIT,3));
        if (i==stepFreqIndex) {
          num=num*0.2;
          while (fabs(num) > 0.4) num = multiplier * CauchyDistribution(); 
        }
        else if (i==maxForwardIndex || i==maxBackIndex) {
          num=num*10.0;           // limits stride length (chasing)
          while (fabs(num) > 20.0 /*|| currentSet.data[i]+num > 80*/) num = 10.0 * multiplier * CauchyDistribution(); 
        } else if (i==fsoIndex) { // limts front side offset to a value > 4 (makes chasing better)
          num=num*5.0;
          while (fabs(num) > 10.0 || currentSet.data[i]+num < 2) num = 5.0 * multiplier * CauchyDistribution(); 
        } else if (i==ffoIndex) { // limts front forward offset to a value > 64 (makes chasing better)
          num=num*5.0;
          while (fabs(num) > 10.0 || currentSet.data[i]+num < 60.0) num = 5.0 * multiplier * CauchyDistribution(); 
        } else if (i==fhIndex) { // limts front height to a value > 62 (makes chasing better)
          num=num*5.0;
          while (fabs(num) > 10.0 || currentSet.data[i]+num < 62.0) num = 5.0 * multiplier * CauchyDistribution(); 
        } else {
          num=num*5.0;
          while (fabs(num) > 10.0) num = 5.0 * multiplier * CauchyDistribution(); 
        }
      }
      //changeSet.data[i] = (1.0-alpha)*num+alpha*changeBestSet.data[i];
      changeSet.data[i] = (alpha*num)+((1.0-alpha)*changeBestSet.data[i]);
      currentSet.data[i] = bestSet.data[i] + changeSet.data[i];   
    }
  }
  cout << "Done Param !\n" << flush;
  // Modify Locus
  double multiplierL = GaussianDistribution(1.0)*(SIG+SIG*pow((double)trials/(double)LIMIT,3));
  if (locusFrontLearning) { 
    for (int i=0; i<9; i++) {
        //double multiplierL = 1.0;
        //if (drand48() >= 0.5) num = 0.0;
        num = CauchyDistribution()*multiplierL;
        while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
       // currentSet.frontLocus[i][0] = bestSet.frontLocus[i][0]+((1.0-alpha)*num+alpha*changeBestSet.frontLocus[i][0]);
        currentSet.frontLocus[i][0] = bestSet.frontLocus[i][0]+(alpha*num+(1.0-alpha)*changeBestSet.frontLocus[i][0]);
        if (drand48() >= 0.5) num = 0.0;
        else num = CauchyDistribution()*multiplierL;
        while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
        //currentSet.frontLocus[i][1] = bestSet.frontLocus[i][1]+((1.0-alpha)*num+alpha*changeBestSet.frontLocus[i][1]);
        currentSet.frontLocus[i][1] = bestSet.frontLocus[i][1]+(alpha*num+(1.0-alpha)*changeBestSet.frontLocus[i][1]);
    
    }
    currentSet.frontLocus[9][0] = currentSet.frontLocus[0][0];
    currentSet.frontLocus[9][1] = currentSet.frontLocus[0][1];
  }
  if (locusBackLearning) { 
    for (int i=0; i<9; i++) {
        //if (drand48() >= 0.5) num = 0.0;
        num = CauchyDistribution()*multiplierL;
        while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();     
        //currentSet.backLocus[i][0] = bestSet.backLocus[i][0]+((1.0-alpha)*num+alpha*changeBestSet.frontLocus[i][0]);
        currentSet.backLocus[i][0] = bestSet.backLocus[i][0]+(alpha*num+(1.0-alpha)*changeBestSet.frontLocus[i][0]);
        if (drand48() >= 0.5) num = 0.0;
        else num = CauchyDistribution()*multiplierL;
        while (fabs(num) > maxLocusChange) num = multiplierL * CauchyDistribution();   
        //currentSet.backLocus[i][1] = bestSet.backLocus[i][1]+((1.0-alpha)*num+alpha*changeBestSet.frontLocus[i][1]);
        currentSet.backLocus[i][1] = bestSet.backLocus[i][1]+(alpha*num+(1.0-alpha)*changeBestSet.frontLocus[i][1]);
    }
    currentSet.backLocus[9][0] = currentSet.backLocus[0][0];
    currentSet.backLocus[9][1] = currentSet.backLocus[0][1];
  }

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

  origFrontLocusPoints_ = currentSet.frontLocus; // set the origFrontLocusPoints_ global to point to the currFrontlocus
  origBackLocusPoints_ = currentSet.backLocus; // set the origBackLocusPoints_ global to point to the currBacklocus
  currentSet.time = 0;
}

double EHCM::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 EHCM::CauchyDistribution() {
  return GaussianDistribution(1.0) / GaussianDistribution(1.0);
}