walker.cc

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

#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];