Tools/Evolution/Parcour.cpp

Go to the documentation of this file.

/**
* @file Parcour.cpp
*
* Implementation of class Parcour.
*
* @author <a href=mailto:dueffert@informatik.hu-berlin.de>Uwe Düffert</a>
*/

#include "Parcour.h"
#include "Platform/SystemCall.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Streams/OutStreams.h"

void Parcour::start()
{
  updateCount=0;
  startTime=SystemCall::getCurrentSystemTime();
  lastTime=startTime;
  minX=-1000;
  maxX=-1000;
  yDiffIntegral=0;
  arcDiffIntegral=0;
  blindCount=0;
  zAccelIntegral=0;
}

void Parcour::update(Pose2D robotPose, Vector3<double> accel, bool blind)
{
  //2do: shouldnt this be done per frame instead of time?
  unsigned long time = SystemCall::getCurrentSystemTime();
  Pose2D pos=getTargetPose(time);
  yDiffIntegral += fabs((pos.translation-robotPose.translation).y);
  arcDiffIntegral += fabs(normalize(pos.rotation-robotPose.rotation));
  blindCount += (int)(blind);
  zAccelIntegral += fabs(accel.z);
  lastRobotPose = robotPose;
  lastTime = time;
  updateCount++;
  if (robotPose.translation.x<minX) { minX=robotPose.translation.x; }
  else if ((robotPose.translation.x>maxX)&&(robotPose.translation.x!=-300)) { maxX=robotPose.translation.x; }
}

Pose2D Parcour::getMotionRequest()
{
  //calculate where we should be in 850ms:
  Pose2D target=getTargetPose(lastTime+850);
  //90 ~ distance of real rotation center to neck joint in mm:
  Pose2D targetCOG=target.minusDiff(Pose2D(0, 90,0));
  Pose2D lastCOG=lastRobotPose.minusDiff(Pose2D(0, 90,0));
  //only correct half of the y difference in 500ms:
  targetCOG.translation.y += (lastCOG.translation.y-targetCOG.translation.y)/7;
  //calculate the difference for our COG between now and in 850ms:
  Pose2D diff=targetCOG-lastCOG;
  
  //calculate speed per s (instead of per 850ms):
  diff.translation /= 0.85;
  diff.rotation /= 0.85;
  return diff;
}

double Parcour::getUnifiedSpeed()
{
  if (lastTime==startTime)
  {
    return 0;
  }
  else
  {
    double speed=1000*(maxX-minX)/(lastTime-startTime);
    double yDiffAvg=yDiffIntegral/updateCount;
    double arcDiffAvg=arcDiffIntegral/updateCount;
    double zAccelAvg=zAccelIntegral/updateCount;
    double blindTime=(double)blindCount/updateCount;
    /*
            ydiff  arcdiff   xAcc        yAcc       zAcc        blind
      fotu  50-85  0.15-0.3  850K-1100K  650K-950K  900K-1300K  0.0-0.015
      back  10-55  0.03-0.35 650K-900K   450K-800K  550K-1150K  0.05-0.15
    */
    double fitness=speed-yDiffAvg/6-arcDiffAvg/0.03-(zAccelAvg-500000)/100000-40*blindTime;
    OUTPUT(idText,text,"speed:" << speed << ", yDiffAvg:" << yDiffAvg << ", arcDiffAvg:" << arcDiffAvg << ", zAccelAvg:" << zAccelAvg << ", blindTime:" << blindTime << " -> fitness:" << fitness);
    OutTextRawFile fitLog("fitness.dat",true);
    fitLog << "speed:" << speed << ", yDiffAvg:" << yDiffAvg << ", arcDiffAvg:" << arcDiffAvg << ", zAccelAvg:" << zAccelAvg << ", blindTime:" << blindTime << " -> fitness:" << fitness << "\n";
    return fitness;
    //    =speed-15-11-8-6 for a bad but still OK run
    //    =speed-8-5-4-0   for a good forward turning run
    //    =speed-2-1-1-2   for a good backward run
  }
}


Pose2D ForwardTurningParcour::getTargetPose(unsigned long targetTime)
{
  Pose2D target;
  target.translation.x=lastRobotPose.translation.x+0.4*(targetTime-lastTime);
  target.rotation=sin((target.translation.x+2600)/200/pi*4)*pi_4;
  if (target.translation.x>-900)
  {
    target.rotation*=max(0.0,(target.translation.x+550.0)/(-450.0));
  }
  //90 ~ distance of real rotation center to neck joint in mm:
  //45mm (instead of 0) makes curve amplitude a little bigger, but yDiff smaller
  target.translation.y=45*sin(target.rotation);
  return target;
}

Pose2D SimpleBackwardParcour::getTargetPose(unsigned long targetTime)
{
  Pose2D target;
  target.translation.x=lastRobotPose.translation.x-0.4*(targetTime-lastTime);
  target.translation.y=0;
  target.rotation=0;
  return target;
}

---