www.pudn.com > Product_Submit2004.rar > EHCM.cc, change:2004-10-12,size:24595b


#include "EHCM.h" 
#include "../TOOLS/BasicTricks.h" 
#include "../TOOLS/MovementTricks.h" 
#include "../TOOLS/HeadTricks.h" 
#include "../Globals.h" 
 
// One of the walk learners 
EHCM::ParameterSet::ParameterSet() { 
  time = 0; 
  speed = 0; 
  for (int j = 0; j<dataSize; j++) data[j]=0.0; 
  frontLocus = new double*[10];  
  backLocus = new double*[10]; 
  for (int i = 0; i < 10; i++) { 
    frontLocus[i] = new double[2]; 
    frontLocus[i][0] = 0.0; 
    frontLocus[i][1] = 0.0; 
    backLocus[i] = new double[2]; 
    backLocus[i][0] = 0.0; 
    backLocus[i][1] = 0.0; 
  } 
} 
 
EHCM::EHCM() { 
  currentTrick = new BasicTricks::NullBody(); 
  headTrick = new BasicTricks::NullHead(); 
  configuration_.ParseFile("/ms/open-r/mw/data/walker.cfg"); 
  alpha = 0.75; 
  inPlaying = false; 
  targetBeacon = vo_greenPinkBeacon_; 
  targetGP = true; 
  startFrame = frame_; 
  endFrame = frame_; 
  runCount = 0; 
 
  trials = 0; 
  successful = 0; 
 
  paramLearning = configuration_.GetAsBool("paramLearning"); 
  locusFrontLearning = configuration_.GetAsBool("locusFrontLearning"); 
  locusBackLearning = configuration_.GetAsBool("locusBackLearning"); 
 
  maxAlpha = configuration_.GetAsDouble("alpha"); 
  decreaseAlpha = configuration_.GetAsBool("decreaseAlpha"); 
  decreaseAlphaRate = configuration_.GetAsDouble("decreaseAlphaRate"); 
 
  multiplier = configuration_.GetAsDouble("multiplier"); 
  maxLocusChange = configuration_.GetAsDouble("maxLocusChange"); 
 
  currentSet.data[0] = configuration_.GetAsDouble("CurrentStepFreq"); 
  currentSet.data[1] = configuration_.GetAsDouble("CurrentMaxForward"); 
  currentSet.data[2] = configuration_.GetAsDouble("CurrentMaxBack"); 
  currentSet.data[3] = configuration_.GetAsDouble("CurrentFSH"); 
  currentSet.data[4] = configuration_.GetAsDouble("CurrentFFO"); 
  currentSet.data[5] = configuration_.GetAsDouble("CurrentFSO"); 
  currentSet.data[6] = configuration_.GetAsDouble("CurrentFH"); 
  currentSet.data[7] = configuration_.GetAsDouble("CurrentBSH"); 
  currentSet.data[8] = configuration_.GetAsDouble("CurrentBFO"); 
  currentSet.data[9] = configuration_.GetAsDouble("CurrentBSO"); 
  currentSet.data[10] = configuration_.GetAsDouble("CurrentBH"); 
 
  currentSet.frontLocus[0][0] = configuration_.GetAsDouble("CurrentFrontX1"); 
  currentSet.frontLocus[0][1] = configuration_.GetAsDouble("CurrentFrontY1"); 
  currentSet.frontLocus[1][0] = configuration_.GetAsDouble("CurrentFrontX2"); 
  currentSet.frontLocus[1][1] = configuration_.GetAsDouble("CurrentFrontY2"); 
  currentSet.frontLocus[2][0] = configuration_.GetAsDouble("CurrentFrontX3"); 
  currentSet.frontLocus[2][1] = configuration_.GetAsDouble("CurrentFrontY3"); 
  currentSet.frontLocus[3][0] = configuration_.GetAsDouble("CurrentFrontX4"); 
  currentSet.frontLocus[3][1] = configuration_.GetAsDouble("CurrentFrontY4"); 
  currentSet.frontLocus[4][0] = configuration_.GetAsDouble("CurrentFrontX5"); 
  currentSet.frontLocus[4][1] = configuration_.GetAsDouble("CurrentFrontY5"); 
  currentSet.frontLocus[5][0] = configuration_.GetAsDouble("CurrentFrontX6"); 
  currentSet.frontLocus[5][1] = configuration_.GetAsDouble("CurrentFrontY6"); 
  currentSet.frontLocus[6][0] = configuration_.GetAsDouble("CurrentFrontX7"); 
  currentSet.frontLocus[6][1] = configuration_.GetAsDouble("CurrentFrontY7"); 
  currentSet.frontLocus[7][0] = configuration_.GetAsDouble("CurrentFrontX8"); 
  currentSet.frontLocus[7][1] = configuration_.GetAsDouble("CurrentFrontY8"); 
  currentSet.frontLocus[8][0] = configuration_.GetAsDouble("CurrentFrontX9"); 
  currentSet.frontLocus[8][1] = configuration_.GetAsDouble("CurrentFrontY9"); 
  currentSet.frontLocus[9][0] = configuration_.GetAsDouble("CurrentFrontX10"); 
  currentSet.frontLocus[9][1] = configuration_.GetAsDouble("CurrentFrontY10"); 
  currentSet.backLocus[0][0] = configuration_.GetAsDouble("CurrentBackX1"); 
  currentSet.backLocus[0][1] = configuration_.GetAsDouble("CurrentBackY1"); 
  currentSet.backLocus[1][0] = configuration_.GetAsDouble("CurrentBackX2"); 
  currentSet.backLocus[1][1] = configuration_.GetAsDouble("CurrentBackY2"); 
  currentSet.backLocus[2][0] = configuration_.GetAsDouble("CurrentBackX3"); 
  currentSet.backLocus[2][1] = configuration_.GetAsDouble("CurrentBackY3"); 
  currentSet.backLocus[3][0] = configuration_.GetAsDouble("CurrentBackX4"); 
  currentSet.backLocus[3][1] = configuration_.GetAsDouble("CurrentBackY4"); 
  currentSet.backLocus[4][0] = configuration_.GetAsDouble("CurrentBackX5"); 
  currentSet.backLocus[4][1] = configuration_.GetAsDouble("CurrentBackY5"); 
  currentSet.backLocus[5][0] = configuration_.GetAsDouble("CurrentBackX6"); 
  currentSet.backLocus[5][1] = configuration_.GetAsDouble("CurrentBackY6"); 
  currentSet.backLocus[6][0] = configuration_.GetAsDouble("CurrentBackX7"); 
  currentSet.backLocus[6][1] = configuration_.GetAsDouble("CurrentBackY7"); 
  currentSet.backLocus[7][0] = configuration_.GetAsDouble("CurrentBackX8"); 
  currentSet.backLocus[7][1] = configuration_.GetAsDouble("CurrentBackY8"); 
  currentSet.backLocus[8][0] = configuration_.GetAsDouble("CurrentBackX9"); 
  currentSet.backLocus[8][1] = configuration_.GetAsDouble("CurrentBackY9"); 
  currentSet.backLocus[9][0] = configuration_.GetAsDouble("CurrentBackX10"); 
  currentSet.backLocus[9][1] = configuration_.GetAsDouble("CurrentBackY10"); 
  currentSet.time = configuration_.GetAsInt("CurrentTime"); 
  currentSet.speed = configuration_.GetAsDouble("CurrentSpeed"); 
 
  bestSet.data[0] = configuration_.GetAsDouble("BestStepFreq"); 
  bestSet.data[1] = configuration_.GetAsDouble("BestMaxForward"); 
  bestSet.data[2] = configuration_.GetAsDouble("BestMaxBack"); 
  bestSet.data[3] = configuration_.GetAsDouble("BestFSH"); 
  bestSet.data[4] = configuration_.GetAsDouble("BestFFO"); 
  bestSet.data[5] = configuration_.GetAsDouble("BestFSO"); 
  bestSet.data[6] = configuration_.GetAsDouble("BestFH"); 
  bestSet.data[7] = configuration_.GetAsDouble("BestBSH"); 
  bestSet.data[8] = configuration_.GetAsDouble("BestBFO"); 
  bestSet.data[9] = configuration_.GetAsDouble("BestBSO"); 
  bestSet.data[10] = configuration_.GetAsDouble("BestBH"); 
   
  bestSet.frontLocus[0][0] = configuration_.GetAsDouble("bestFrontX1"); 
  bestSet.frontLocus[0][1] = configuration_.GetAsDouble("bestFrontY1"); 
  bestSet.frontLocus[1][0] = configuration_.GetAsDouble("bestFrontX2"); 
  bestSet.frontLocus[1][1] = configuration_.GetAsDouble("bestFrontY2"); 
  bestSet.frontLocus[2][0] = configuration_.GetAsDouble("bestFrontX3"); 
  bestSet.frontLocus[2][1] = configuration_.GetAsDouble("bestFrontY3"); 
  bestSet.frontLocus[3][0] = configuration_.GetAsDouble("bestFrontX4"); 
  bestSet.frontLocus[3][1] = configuration_.GetAsDouble("bestFrontY4"); 
  bestSet.frontLocus[4][0] = configuration_.GetAsDouble("bestFrontX5"); 
  bestSet.frontLocus[4][1] = configuration_.GetAsDouble("bestFrontY5"); 
  bestSet.frontLocus[5][0] = configuration_.GetAsDouble("bestFrontX6"); 
  bestSet.frontLocus[5][1] = configuration_.GetAsDouble("bestFrontY6"); 
  bestSet.frontLocus[6][0] = configuration_.GetAsDouble("bestFrontX7"); 
  bestSet.frontLocus[6][1] = configuration_.GetAsDouble("bestFrontY7"); 
  bestSet.frontLocus[7][0] = configuration_.GetAsDouble("bestFrontX8"); 
  bestSet.frontLocus[7][1] = configuration_.GetAsDouble("bestFrontY8"); 
  bestSet.frontLocus[8][0] = configuration_.GetAsDouble("bestFrontX9"); 
  bestSet.frontLocus[8][1] = configuration_.GetAsDouble("bestFrontY9"); 
  bestSet.frontLocus[9][0] = configuration_.GetAsDouble("bestFrontX10"); 
  bestSet.frontLocus[9][1] = configuration_.GetAsDouble("bestFrontY10"); 
  bestSet.backLocus[0][0] = configuration_.GetAsDouble("bestBackX1"); 
  bestSet.backLocus[0][1] = configuration_.GetAsDouble("bestBackY1"); 
  bestSet.backLocus[1][0] = configuration_.GetAsDouble("bestBackX2"); 
  bestSet.backLocus[1][1] = configuration_.GetAsDouble("bestBackY2"); 
  bestSet.backLocus[2][0] = configuration_.GetAsDouble("bestBackX3"); 
  bestSet.backLocus[2][1] = configuration_.GetAsDouble("bestBackY3"); 
  bestSet.backLocus[3][0] = configuration_.GetAsDouble("bestBackX4"); 
  bestSet.backLocus[3][1] = configuration_.GetAsDouble("bestBackY4"); 
  bestSet.backLocus[4][0] = configuration_.GetAsDouble("bestBackX5"); 
  bestSet.backLocus[4][1] = configuration_.GetAsDouble("bestBackY5"); 
  bestSet.backLocus[5][0] = configuration_.GetAsDouble("bestBackX6"); 
  bestSet.backLocus[5][1] = configuration_.GetAsDouble("bestBackY6"); 
  bestSet.backLocus[6][0] = configuration_.GetAsDouble("bestBackX7"); 
  bestSet.backLocus[6][1] = configuration_.GetAsDouble("bestBackY7"); 
  bestSet.backLocus[7][0] = configuration_.GetAsDouble("bestBackX8"); 
  bestSet.backLocus[7][1] = configuration_.GetAsDouble("bestBackY8"); 
  bestSet.backLocus[8][0] = configuration_.GetAsDouble("bestBackX9"); 
  bestSet.backLocus[8][1] = configuration_.GetAsDouble("bestBackY9"); 
  bestSet.backLocus[9][0] = configuration_.GetAsDouble("bestBackX10"); 
  bestSet.backLocus[9][1] = configuration_.GetAsDouble("bestBackY10"); 
 
  bestSet.time = configuration_.GetAsInt("BestTime"); 
  bestSet.speed = configuration_.GetAsDouble("BestSpeed"); 
 
  lastSet = currentSet; 
 
  deltaSpeedC = 1; 
  alpha = maxAlpha; 
 
  beaconDistance = 0.0; 
  beaconCount = 0; 
  prevBeaconDistance = 0.0; 
  prevWalkDistance = 0.0; 
} 
 
EHCM::~EHCM() { 
  delete currentTrick; 
  delete headTrick; 
} 
 
void EHCM::NewReadyModel() { 
  if (targetGP) targetBeacon = vo_greenPinkBeacon_; 
  else targetBeacon = vo_pinkGreenBeacon_; 
 
  if (inPlaying) { 
    delete headTrick; 
    headTrick = new HeadTricks::HeadPan(85,-85,15,50); 
    headTrick->Start(); 
  } 
 
  int hC=headTrick->Continue(); 
  if (hC >= 1 && strcmp("HeadPan",headTrick->GetName())==0) { 
    if (targetBeacon != NULL) { 
      delete headTrick; 
      headTrick = new HeadTricks::FollowBeaconWithHeadSticky(10,&targetBeacon); 
      headTrick->Start(); 
    } 
  } else if (hC < 1) { 
    delete headTrick; 
    headTrick = new HeadTricks::HeadPan(85,-85,10,50); 
    headTrick->Start(); 
  } 
   
  inPlaying = false; 
} 
 
void EHCM::NewPlayingModel() { 
  if (targetGP) targetBeacon = vo_greenPinkBeacon_; 
  else targetBeacon = vo_pinkGreenBeacon_; 
   
  if (!inPlaying) { 
    srand(frame_); 
    srand48(frame_); 
    delete currentTrick; 
    currentTrick = new MovementTricks::FaceBeacon(&targetBeacon,!targetGP); 
    currentTrick->Start(); 
    inPlaying = true; 
    return; 
  }  
   
  int cC = currentTrick->Continue(); 
  if (cC < 1 && strcmp("FaceBeacon",currentTrick->GetName())==0) { 
    delete currentTrick; 
    currentTrick = new MovementTricks::ChaseBeacon(&targetBeacon); 
    StartTimer(); 
    currentTrick->Start(); 
  } else if (cC < 1 && strcmp("ChaseBeacon",currentTrick->GetName())==0) { 
    if (cC == 0) EndTimer(true); else EndTimer(false); 
    utils.Kick(99,false); // GetUp 
    delete currentTrick; 
    currentTrick = new BasicTricks::NullBodyTime(15); 
    currentTrick->Start();     
     
    beaconDistance = 0; 
    beaconCount = 0; 
  } else if (cC < 1 && strcmp("NullBodyTime",currentTrick->GetName())==0) { 
    NullBodyy(); 
    CheckImprovement(); 
 
    delete currentTrick; 
    currentTrick = new MovementTricks::FaceBeacon(&targetBeacon,!targetGP); 
    currentTrick->Start(); 
  }  else if (cC > 0 && strcmp("NullBodyTime",currentTrick->GetName())==0) { 
    if (!targetGP) { 
      if (vo_pinkGreenBeacon_!=NULL) { 
        beaconDistance+=vo_pinkGreenBeacon_->distance_; 
        beaconCount++; 
      }  
    } else {  
      if (vo_greenPinkBeacon_!=NULL) { 
        beaconDistance+=vo_greenPinkBeacon_->distance_; 
        beaconCount++; 
      } 
    } 
  } 
} 
 
void EHCM::NullBodyy() { 
   
     // 13 is loss in distance caused by dog turning around 
    // 9 is a vision fudge factor on ERS 7, 16 on 210 
    double fudge = 9.0; 
    double frate = 30.0; 
    #ifdef ERS_210 
      fudge = 16; 
      frate = 25.0; 
    #endif 
    if (beaconCount == 0) beaconCount = 1; 
    beaconDistance = beaconDistance / beaconCount; 
    double walkDistance = (290.0-beaconDistance-prevBeaconDistance-13.0+fudge); 
    double newspeed = 0.0; 
    if (targetGP) { 
      newspeed = (walkDistance+prevWalkDistance)/(currentSet.time/frate); 
      walkDistance = walkDistance+prevWalkDistance; 
    } 
    else 
      newspeed = (walkDistance)/(currentSet.time/frate); 
    
    currentSet.speed = newspeed; 
     
    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); 
}