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


#include "Walker.h" 
#include "../TOOLS/BasicTricks.h" 
#include "../TOOLS/MovementTricks.h" 
#include "../TOOLS/HeadTricks.h" 
#include "../Globals.h" 
 
 
// Another walk learner. 
Walker::Walker() { 
  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; 
 
  learnType = HILLCLIMBING2; 
 
  currFrontLocus = new double*[10]; 
  currBackLocus = new double*[10]; 
  bestFrontLocus = new double*[10];  
  bestBackLocus = new double*[10]; 
  for (int i = 0; i < 10; i++) { 
    currFrontLocus[i] = new double[2]; 
    currBackLocus[i] = new double[2]; 
    bestFrontLocus[i] = new double[2]; 
    bestBackLocus[i] = new double[2]; 
  } 
 
  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"); 
 
  currFrontLocus[0][0] = configuration_.GetAsDouble("CurrentFrontX1"); 
  currFrontLocus[0][1] = configuration_.GetAsDouble("CurrentFrontY1"); 
  currFrontLocus[1][0] = configuration_.GetAsDouble("CurrentFrontX2"); 
  currFrontLocus[1][1] = configuration_.GetAsDouble("CurrentFrontY2"); 
  currFrontLocus[2][0] = configuration_.GetAsDouble("CurrentFrontX3"); 
  currFrontLocus[2][1] = configuration_.GetAsDouble("CurrentFrontY3"); 
  currFrontLocus[3][0] = configuration_.GetAsDouble("CurrentFrontX4"); 
  currFrontLocus[3][1] = configuration_.GetAsDouble("CurrentFrontY4"); 
  currFrontLocus[4][0] = configuration_.GetAsDouble("CurrentFrontX5"); 
  currFrontLocus[4][1] = configuration_.GetAsDouble("CurrentFrontY5"); 
  currFrontLocus[5][0] = configuration_.GetAsDouble("CurrentFrontX6"); 
  currFrontLocus[5][1] = configuration_.GetAsDouble("CurrentFrontY6"); 
  currFrontLocus[6][0] = configuration_.GetAsDouble("CurrentFrontX7"); 
  currFrontLocus[6][1] = configuration_.GetAsDouble("CurrentFrontY7"); 
  currFrontLocus[7][0] = configuration_.GetAsDouble("CurrentFrontX8"); 
  currFrontLocus[7][1] = configuration_.GetAsDouble("CurrentFrontY8"); 
  currFrontLocus[8][0] = configuration_.GetAsDouble("CurrentFrontX9"); 
  currFrontLocus[8][1] = configuration_.GetAsDouble("CurrentFrontY9"); 
  currFrontLocus[9][0] = configuration_.GetAsDouble("CurrentFrontX10"); 
  currFrontLocus[9][1] = configuration_.GetAsDouble("CurrentFrontY10"); 
  currBackLocus[0][0] = configuration_.GetAsDouble("CurrentBackX1"); 
  currBackLocus[0][1] = configuration_.GetAsDouble("CurrentBackY1"); 
  currBackLocus[1][0] = configuration_.GetAsDouble("CurrentBackX2"); 
  currBackLocus[1][1] = configuration_.GetAsDouble("CurrentBackY2"); 
  currBackLocus[2][0] = configuration_.GetAsDouble("CurrentBackX3"); 
  currBackLocus[2][1] = configuration_.GetAsDouble("CurrentBackY3"); 
  currBackLocus[3][0] = configuration_.GetAsDouble("CurrentBackX4"); 
  currBackLocus[3][1] = configuration_.GetAsDouble("CurrentBackY4"); 
  currBackLocus[4][0] = configuration_.GetAsDouble("CurrentBackX5"); 
  currBackLocus[4][1] = configuration_.GetAsDouble("CurrentBackY5"); 
  currBackLocus[5][0] = configuration_.GetAsDouble("CurrentBackX6"); 
  currBackLocus[5][1] = configuration_.GetAsDouble("CurrentBackY6"); 
  currBackLocus[6][0] = configuration_.GetAsDouble("CurrentBackX7"); 
  currBackLocus[6][1] = configuration_.GetAsDouble("CurrentBackY7"); 
  currBackLocus[7][0] = configuration_.GetAsDouble("CurrentBackX8"); 
  currBackLocus[7][1] = configuration_.GetAsDouble("CurrentBackY8"); 
  currBackLocus[8][0] = configuration_.GetAsDouble("CurrentBackX9"); 
  currBackLocus[8][1] = configuration_.GetAsDouble("CurrentBackY9"); 
  currBackLocus[9][0] = configuration_.GetAsDouble("CurrentBackX10"); 
  currBackLocus[9][1] = configuration_.GetAsDouble("CurrentBackY10"); 
  currentSet.time = configuration_.GetAsInt("CurrentTime");; 
 
  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"); 
   
  bestFrontLocus[0][0] = configuration_.GetAsDouble("bestFrontX1"); 
  bestFrontLocus[0][1] = configuration_.GetAsDouble("bestFrontY1"); 
  bestFrontLocus[1][0] = configuration_.GetAsDouble("bestFrontX2"); 
  bestFrontLocus[1][1] = configuration_.GetAsDouble("bestFrontY2"); 
  bestFrontLocus[2][0] = configuration_.GetAsDouble("bestFrontX3"); 
  bestFrontLocus[2][1] = configuration_.GetAsDouble("bestFrontY3"); 
  bestFrontLocus[3][0] = configuration_.GetAsDouble("bestFrontX4"); 
  bestFrontLocus[3][1] = configuration_.GetAsDouble("bestFrontY4"); 
  bestFrontLocus[4][0] = configuration_.GetAsDouble("bestFrontX5"); 
  bestFrontLocus[4][1] = configuration_.GetAsDouble("bestFrontY5"); 
  bestFrontLocus[5][0] = configuration_.GetAsDouble("bestFrontX6"); 
  bestFrontLocus[5][1] = configuration_.GetAsDouble("bestFrontY6"); 
  bestFrontLocus[6][0] = configuration_.GetAsDouble("bestFrontX7"); 
  bestFrontLocus[6][1] = configuration_.GetAsDouble("bestFrontY7"); 
  bestFrontLocus[7][0] = configuration_.GetAsDouble("bestFrontX8"); 
  bestFrontLocus[7][1] = configuration_.GetAsDouble("bestFrontY8"); 
  bestFrontLocus[8][0] = configuration_.GetAsDouble("bestFrontX9"); 
  bestFrontLocus[8][1] = configuration_.GetAsDouble("bestFrontY9"); 
  bestFrontLocus[9][0] = configuration_.GetAsDouble("bestFrontX10"); 
  bestFrontLocus[9][1] = configuration_.GetAsDouble("bestFrontY10"); 
  bestBackLocus[0][0] = configuration_.GetAsDouble("bestBackX1"); 
  bestBackLocus[0][1] = configuration_.GetAsDouble("bestBackY1"); 
  bestBackLocus[1][0] = configuration_.GetAsDouble("bestBackX2"); 
  bestBackLocus[1][1] = configuration_.GetAsDouble("bestBackY2"); 
  bestBackLocus[2][0] = configuration_.GetAsDouble("bestBackX3"); 
  bestBackLocus[2][1] = configuration_.GetAsDouble("bestBackY3"); 
  bestBackLocus[3][0] = configuration_.GetAsDouble("bestBackX4"); 
  bestBackLocus[3][1] = configuration_.GetAsDouble("bestBackY4"); 
  bestBackLocus[4][0] = configuration_.GetAsDouble("bestBackX5"); 
  bestBackLocus[4][1] = configuration_.GetAsDouble("bestBackY5"); 
  bestBackLocus[5][0] = configuration_.GetAsDouble("bestBackX6"); 
  bestBackLocus[5][1] = configuration_.GetAsDouble("bestBackY6"); 
  bestBackLocus[6][0] = configuration_.GetAsDouble("bestBackX7"); 
  bestBackLocus[6][1] = configuration_.GetAsDouble("bestBackY7"); 
  bestBackLocus[7][0] = configuration_.GetAsDouble("bestBackX8"); 
  bestBackLocus[7][1] = configuration_.GetAsDouble("bestBackY8"); 
  bestBackLocus[8][0] = configuration_.GetAsDouble("bestBackX9"); 
  bestBackLocus[8][1] = configuration_.GetAsDouble("bestBackY9"); 
  bestBackLocus[9][0] = configuration_.GetAsDouble("bestBackX10"); 
  bestBackLocus[9][1] = configuration_.GetAsDouble("bestBackY10"); 
 
  bestSet.time = configuration_.GetAsInt("BestTime"); 
 
  for (int i=0; i<dataSize; i++) { 
    change.data[i]=0.0; 
    lastSet.data[i]=currentSet.data[i]; 
  } 
  change.time = 0; 
  lastSet.time = 0; 
  alpha = maxAlpha; 
 
  beaconDistance = 0.0; 
  beaconCount = 0; 
  prevBeaconDistance = 0.0; 
} 
 
Walker::~Walker() { 
  delete currentTrick; 
  delete headTrick; 
} 
 
void Walker::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 Walker::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); 
    targetGP = !targetGP; // switch beacon target 
    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) { 
    // 13 is loss in distance caused by dog turning around 
    // 9 is a vision fudge factor 
    beaconDistance = beaconDistance / beaconCount; 
    double walkDistance = (290.0-beaconDistance-prevBeaconDistance-13.0+9.0); 
    if (!targetGP) 
      cout << "Walk Distance = " << walkDistance+prevWalkDistance << "     Speed = " <<  (walkDistance+prevWalkDistance)/(lastSet.time/30.0) << "    Beacon Distance = " << beaconDistance << " (" << beaconCount << ")" << endl << flush; 
    else 
      cout << "Walk Distance = " << walkDistance << "     Speed = " <<  (walkDistance)/(currentSet.time/30.0) << "    Beacon Distance = " << beaconDistance << " (" << beaconCount << ")" << endl << flush; 
    prevBeaconDistance = beaconDistance; 
    prevWalkDistance=walkDistance; 
    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 Walker::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 Walker::EndTimer(bool worked) { 
  endFrame = frame_; 
  double numSteps = lcq_.GetNumSteps(); 
  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"; 
  if (targetGP) { // Only learn after going both directions .. there seems to be a difference 
    if (runCount > 0) { 
      walkP_.run = runCount; 
      walkP_.time = currentSet.time; 
      for (int j=0; j<dataSize; j++) {  
        walkP_.data[j] = currentSet.data[j]; 
      } 
      for (int i=0; i<10; i++) { 
          walkP_.frontXpoints[i] = currFrontLocus[i][0]; 
          walkP_.frontYpoints[i] = currFrontLocus[i][1]; 
          walkP_.backXpoints[i] = currBackLocus[i][0]; 
          walkP_.backYpoints[i] = currBackLocus[i][1]; 
      } 
      walkP_.add = true; 
 
      if (currentSet.time < bestSet.time && currentSet.time > 150) { 
        for (int i=0; i<dataSize; i++) bestSet.data[i]=currentSet.data[i]; 
        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); 
}