ehcm.cc

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

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