Modules/BehaviorControl/GT2004BehaviorControl/GT2004BasicBehaviors/GT2004EvolutionBasicBehaviors.cpp

Go to the documentation of this file.

/** 
* @file GT2004EvolutionBasicBehaviors.cpp
*
* Implementation of basic behaviors defined in evolution-basic-behaviors.xml.
*
* @author Uwe Düffert
*/

#include "Tools/Math/PIDsmoothedValue.h"
#include "GT2004EvolutionBasicBehaviors.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Math/Common.h"
#include "Tools/Streams/OutStreams.h"

#include "Tools/Debugging/Debugging.h"
#include "Platform/GTAssert.h"

void GT2004EvolutionBasicBehaviors::registerBasicBehaviors(Xabsl2Engine& engine)
{
  engine.registerBasicBehavior(basicBehaviorEvolveOmniParameters);
  engine.registerBasicBehavior(basicBehaviorMeasureGT2004Parameters);
  engine.registerBasicBehavior(basicBehaviorMeasureGT2004ParametersBlind);
  engine.registerBasicBehavior(basicBehaviorSendCurrentGT2004ParametersAndChooseNext);
  engine.registerBasicBehavior(basicBehaviorNextGT2004ParametersToBeMeasured);
}

void GT2004BasicBehaviorEvolveOmniParameters::execute()
{
  switch ((int)mode)
  {
  case 0: //walk parcour forward
    if (lastMode!=0) forwardTurningParcour.start();
    forwardTurningParcour.update(robotPose, robotState.acceleration, robotPose.getValidity()<0.5);
    motionRequest.motionType = MotionRequest::walk;
    motionRequest.walkRequest.walkType = WalkRequest::normal;
    motionRequest.walkRequest.walkParams=forwardTurningParcour.getMotionRequest();
    motionRequest.updateRP=false;
/*
    if (lastMode!=0)
    {
      motionRequestX.resetTo(motionRequest.walkRequest.walkParams.translation.x); 
      motionRequestY.resetTo(motionRequest.walkRequest.walkParams.translation.y);
      motionRequestR.resetTo(motionRequest.walkRequest.walkParams.rotation);
    }
    else
    {
      motionRequestX.approximateVal(motionRequest.walkRequest.walkParams.translation.x); 
      motionRequestY.approximateVal(motionRequest.walkRequest.walkParams.translation.y);
      motionRequestR.approximateVal(motionRequest.walkRequest.walkParams.rotation);
    }
    motionRequest.walkRequest.walkParams.translation.x=motionRequestX.getVal();
    motionRequest.walkRequest.walkParams.translation.y=motionRequestY.getVal();
    motionRequest.walkRequest.walkParams.rotation=motionRequestR.getVal();
*/
    break;
  case 1: //walk parcour backward
    if (lastMode!=1)
    {
      if (evolutionMode!=0)
      {
        GT2004Parameters* param=bPopulation.getNextIndividualWithoutFitness();
        //send it to Motion:
        gt2004Parameters = *param;
        invKinWalkingParameters = gt2004Parameters;
        walkParameterTimeStamp=SystemCall::getCurrentSystemTime();
        //send it as DebugMessage too to notify RobotControl dialogs:
        OUTPUT(idInvKinWalkingParameters,bin,invKinWalkingParameters);
        OUTPUT(idGT2004Parameters,bin,gt2004Parameters);
      }
      simpleBackwardParcour.start();
    }
    simpleBackwardParcour.update(robotPose, robotState.acceleration, robotPose.getValidity()<0.5);
    motionRequest.motionType = MotionRequest::walk;
    motionRequest.walkRequest.walkType = WalkRequest::normal;
    motionRequest.walkRequest.walkParams=simpleBackwardParcour.getMotionRequest();
    motionRequest.updateRP=false;
/*
    if (lastMode!=1)
    {
      motionRequestX.resetTo(motionRequest.walkRequest.walkParams.translation.x); 
      motionRequestY.resetTo(motionRequest.walkRequest.walkParams.translation.y);
      motionRequestR.resetTo(motionRequest.walkRequest.walkParams.rotation);
    }
    else
    {
      motionRequestX.approximateVal(motionRequest.walkRequest.walkParams.translation.x); 
      motionRequestY.approximateVal(motionRequest.walkRequest.walkParams.translation.y);
      motionRequestR.approximateVal(motionRequest.walkRequest.walkParams.rotation);
    }
    motionRequest.walkRequest.walkParams.translation.x=motionRequestX.getVal();
    motionRequest.walkRequest.walkParams.translation.y=motionRequestY.getVal();
    motionRequest.walkRequest.walkParams.rotation=motionRequestR.getVal();
*/
    break;
  case 2: //position
    {
      if ((lastMode!=2)&&(evolutionMode!=0))
      {
        //initialize rand(), but only if we dont want direct comparisons anymore
        //2do: is this better than in ...next...()?
        //srand(SystemCall::getCurrentSystemTime());
        
        ERS7EvolveWalkingParameters par;
        invKinWalkingParameters = par;
        gt2004Parameters = invKinWalkingParameters;
        walkParameterTimeStamp=SystemCall::getCurrentSystemTime();
      }  
      //this is good old goToPoint:
      Vector2<double> destination(-2600,0);
      const double destinationAngle=0;
      const double maxSpeed = 300;
      const double maxTurnSpeed = fromDegrees(40);
      const Vector2<double>& self = robotPose.translation;
      double distanceToDestination = Geometry::distanceTo(self,destination);
      double angleDifference = normalize(fromDegrees(destinationAngle) - robotPose.rotation);
      double angleToDestination = Geometry::angleTo(robotPose,destination);
      motionRequest.motionType = MotionRequest::walk;
      motionRequest.walkRequest.walkType = WalkRequest::normal;
      //this time does not necessarily inclgt2004e time for turning!:
      double estimatedTimeToReachDestination;
      if (distanceToDestination > 200)
      {
        estimatedTimeToReachDestination = (distanceToDestination+200)/ maxSpeed;
        
        motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
        motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
      }
      else
      {
        estimatedTimeToReachDestination = 2*distanceToDestination / maxSpeed;
        if (distanceToDestination > 10)
        {
          motionRequest.walkRequest.walkParams.translation.x = 
            cos(angleToDestination) * maxSpeed*distanceToDestination/200;
          motionRequest.walkRequest.walkParams.translation.y = 
            sin(angleToDestination) * maxSpeed*distanceToDestination/200;
        }
        else
        {
          motionRequest.walkRequest.walkParams.translation.x = 0;
          motionRequest.walkRequest.walkParams.translation.y = 0;
        }
      }
      if (estimatedTimeToReachDestination==0)
      {
        estimatedTimeToReachDestination = 0.001;
      }
      if (fabs(toDegrees(angleDifference)) > 20)
      {
        motionRequest.walkRequest.walkParams.rotation = 
          angleDifference / estimatedTimeToReachDestination;
        if (motionRequest.walkRequest.walkParams.rotation > maxTurnSpeed)
        {
          motionRequest.walkRequest.walkParams.rotation = maxTurnSpeed;
        }
        if (motionRequest.walkRequest.walkParams.rotation < -maxTurnSpeed) 
        {
          motionRequest.walkRequest.walkParams.rotation = -maxTurnSpeed;
        }
      }
      else
      {
        motionRequest.walkRequest.walkParams.rotation = 2*angleDifference;
      }
      if ((fabs(toDegrees(angleDifference))<4)&&(distanceToDestination<25))
      {
        motionRequest.walkRequest.walkParams.translation.x = 0;
        motionRequest.walkRequest.walkParams.translation.y = 0;
        motionRequest.walkRequest.walkParams.rotation = 0;
      }
      if (fabs(motionRequest.walkRequest.walkParams.translation.x) < 10
        && fabs(motionRequest.walkRequest.walkParams.translation.y) < 10
        && fabs(motionRequest.walkRequest.walkParams.rotation) < fromDegrees(10)
        && fabs(distanceToDestination) < 40
        && fabs(angleToDestination) < fromDegrees(5))
      {
        motionRequest.motionType = MotionRequest::stand;
      }
    }
    break;
  case 3: //evolve walking parameters (choose next; called when ready)
    motionRequest.motionType = MotionRequest::stand;
    if ((lastMode!=3)&&(evolutionMode!=0))
    {
      GT2004Parameters* param=bPopulation.getNextIndividualWithoutFitness();
      while (param==0)
      {
        //there is no unevaluated individual anymore, so lets build the next generation
        OUTPUT(idText,text, "Backward_Evolution!");
        //the worse half of the population shall be replaced by mutations/crossovers:
        bPopulation.evolve(0.5, 0.6, 0.3, 0.12, true);
        param=bPopulation.getNextIndividualWithoutFitness();
      }
      
      param=ftPopulation.getNextIndividualWithoutFitness();
      while (param==0)
      {
        //there is no unevaluated individual anymore, so lets build the next generation
        OUTPUT(idText,text, "ForwardTurning_Evolution!");
        //the worse half of the population shall be replaced by mutations/crossovers:
        ftPopulation.evolve(0.5, 0.6, 0.3, 0.12, true);
        param=ftPopulation.getNextIndividualWithoutFitness();
      }
      
      //send it to Motion:
      gt2004Parameters = *param;
      invKinWalkingParameters = gt2004Parameters;
      walkParameterTimeStamp=SystemCall::getCurrentSystemTime();
      //send it as DebugMessage too to notify RobotControl dialogs:
      OUTPUT(idInvKinWalkingParameters,bin,invKinWalkingParameters);
      OUTPUT(idGT2004Parameters,bin,gt2004Parameters);
    }
    break;
  case 4: //abort (individual failed)
    motionRequest.motionType = MotionRequest::stand;
    if (lastMode!=4)
    {
      GT2004Parameters* param=ftPopulation.getNextIndividualWithoutFitness();
      if (param!=0)
      {
        param->fitness=0;
        
        OutTextRawFile fitLog("fitness.dat",true);
        fitLog << "FTFitness = 0\n";
        if (evolutionMode!=0)
        {
          ftPopulation.outputStatistics(&fitLog);
        }
        else
        {
          param->fitness=-1;
        }
      }
      param=bPopulation.getNextIndividualWithoutFitness();
      if (param!=0)
      {
        param->fitness=0;
        
        OutTextRawFile fitLog("fitness.dat",true);
        fitLog << "BFitness = 0\n";
        if (evolutionMode!=0)
        {
          bPopulation.outputStatistics(&fitLog);
        }
        else
        {
          param->fitness=-1;
        }
      }
      OUTPUT(idText,text, "Fitness = 0");
    }
    break;
  case 5: //calc fitness (only called after complete run: no abort, no pause)
    motionRequest.motionType = MotionRequest::stand;
    if (lastMode!=5)
    {
      GT2004Parameters* param=ftPopulation.getNextIndividualWithoutFitness();
      if (param!=0)
      {
        param->fitness=forwardTurningParcour.getUnifiedSpeed();
        OutTextRawFile fitLog("fitness.dat",true);
        fitLog << "FTFitness = " << param->fitness << "\n";
        if (evolutionMode!=0)
        {
          ftPopulation.outputStatistics(&fitLog);
        
          OutTextFile outFile("ft_opt.dat",true);
          outFile << *param;
          outFile << param->fitness;
        }
        else
        {
          param->fitness=-1;
        }
      }
      GT2004Parameters* bparam=bPopulation.getNextIndividualWithoutFitness();
      if (bparam!=0)
      {
        bparam->fitness=simpleBackwardParcour.getUnifiedSpeed();
        OutTextRawFile fitLog("fitness.dat",true);
        fitLog << "BFitness = " << bparam->fitness << "\n";
        if (evolutionMode!=0)
        {
          bPopulation.outputStatistics(&fitLog);
        
          OutTextFile outFile("b_opt.dat",true);
          outFile << *bparam;
          outFile << bparam->fitness;
        }
        else
        {
          param->fitness=-1;
        }
      }
      OutTextRawFile fitLog("fitness.dat",true);
      fitLog << "Fitness = " << 0.67*param->fitness+0.33*bparam->fitness << "\n";
      OUTPUT(idText,text, "Fitness = " << 0.67*param->fitness+0.33*bparam->fitness);

      /*
      OutTextFile outFTPopul("ft_pop.dat");
      for (int i=0;i<10;i++)
      {
      outFTPopul << *ftPopulation.individual[i];
      outFTPopul << ftPopulation.individual[i]->fitness;
      }
      OutTextFile outBPopul("b_pop.dat");
      for (int i=0;i<10;i++)
      {
      outBPopul << *bPopulation.individual[i];
      outBPopul << bPopulation.individual[i]->fitness;
      }
      */
    }
    break;
  }
  lastMode=(int)mode;
}


void GT2004BasicBehaviorMeasureGT2004Parameters::execute()
{
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  motionRequest.walkRequest.walkParams = measurementRequest;
  
  if ((robotPose.frameNumber-lastFrameNumber>250)||(robotPose.frameNumber<lastFrameNumber))
  {
    //if last robot pose is older than 2s, this basic behavior was restarted
    startFrameNumber=robotPose.frameNumber;
    char txt[512];
    sprintf(txt," (%3.1f, %3.1f, %1.3f)",motionRequest.walkRequest.walkParams.translation.x,motionRequest.walkRequest.walkParams.translation.y,motionRequest.walkRequest.walkParams.rotation);
    GT2004Parameters* param= gt2004ParametersSet.getParameters(gt2004CurrentIndex);
    if ((gt2004CurrentIndex<(int)GT2004ParametersSet::numberOfParameters)&&(evolutionMode!=0))
    {
      //send the parameters to be measured to motion, just to be sure it has the correct ones
      gt2004Parameters = *param;
      walkParameterTimeStamp=SystemCall::getCurrentSystemTime();
    }
    if (gt2004CurrentIndex<(int)GT2004ParametersSet::numberOfParameters)
    {
      sprintf(txt," (%3.1f, %3.1f, %1.3f), corrected [%3.1f, %3.1f, %1.3f]:",motionRequest.walkRequest.walkParams.translation.x,motionRequest.walkRequest.walkParams.translation.y,motionRequest.walkRequest.walkParams.rotation,param->correctedMotion.translation.x,param->correctedMotion.translation.y,param->correctedMotion.rotation);
    }
    OUTPUT(idText,text,"measure " << GT2004ParametersSet::getIndexString(gt2004CurrentIndex) << txt);
    //send them as debug message too to notify GT2004ParametersDialog
    OUTPUT(idGT2004Parameters,bin,((gt2004CurrentIndex<GT2004ParametersSet::numberOfParameters)?*gt2004ParametersSet.getParameters(gt2004CurrentIndex):gt2004ExtraParameters));
  }
  lastFrameNumber=robotPose.frameNumber;
  
  //look at MotionRequestResult.nb to understand the following calculation
  /*
  Step0:
  start with (x0,y0,r0) and walk until (xt,yt,r(t)),
  the following calculation is only valid for the point the robot turns around!
  Use that point for x/y here!
  */

  //start point is last valid pose in 2.0s after basic behavior start:
  if ((robotPose.frameNumber-startFrameNumber<250)&&(robotPose.getValidity()>0.5))
  {
    //we have to use COG/point of turn for x/y here:
    x0=robotPose.translation.x-100*cos(robotPose.rotation);
    y0=robotPose.translation.y-100*sin(robotPose.rotation);
    r0=robotPose.rotation;
    t0=0.008*robotPose.frameNumber;
  }
  
  //end point is last valid pose after at least 4.0s:
  if ((robotPose.frameNumber-startFrameNumber>500)&&(robotPose.getValidity()>0.5))
  {
    double t=0.008*robotPose.frameNumber-t0;
    //we have to use COG/point of turn for x/y here:
    double xt=robotPose.translation.x-100*cos(robotPose.rotation);
    double yt=robotPose.translation.y-100*sin(robotPose.rotation);
    double rt=robotPose.rotation;
  
    //Step3: calculate dr=(r(t)-r0)/t
    double dr,dx,dy;
    dr=(rt-r0)/t;

    //2do: ensure dr*t != n*pi
    if (fabs(dr)>0.001)
    {
      //Step4.1 (if dr!=0):
      double cdrt2=cos(r0+dr*t/2);
      double sdrt2=sin(r0+dr*t/2);
      dx=dr/2/sin(dr*t/2)*((xt-x0)*cdrt2+(yt-y0)*sdrt2);
      dy=dr*sin(dr*t/2)*((y0-yt)*cdrt2+(xt-x0)*sdrt2)/(cos(dr*t)-1);
    }
    else
    {
      //Step4.2 (if dr==0):
      dx=((xt-x0)*cos(r0)+(yt-y0)*sin(r0))/t;
      dy=((yt-y0)*cos(r0)+(x0-xt)*sin(r0))/t;
    }
    //CARE!: this is NOT position change of neck but of COG!:
    gt2004ParametersCalibration[gt2004CurrentIndex]=Pose2D(dr,dx,dy);
  }
}


void GT2004BasicBehaviorMeasureGT2004ParametersBlind::execute()
{
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  motionRequest.walkRequest.walkParams = measurementRequest;
  if (fabs(measurementRequest.rotation)>1.2)
  {
    headControlMode.headControlMode=HeadControlMode::lookParallelToGround;
  }
  else
  {
    headControlMode.headControlMode=HeadControlMode::watchOrigin;
  }
  
  char txt[512];
  if ((robotPose.frameNumber-lastFrameNumber>250)||(robotPose.frameNumber<lastFrameNumber))
  {
    //if last robot pose is older than 2s, this basic behavior was restarted
    startFrameNumber=robotPose.frameNumber;
    sprintf(txt," (%3.1f, %3.1f, %1.3f)",motionRequest.walkRequest.walkParams.translation.x,motionRequest.walkRequest.walkParams.translation.y,motionRequest.walkRequest.walkParams.rotation);
    GT2004Parameters* param= gt2004ParametersSet.getParameters(gt2004CurrentIndex);
    if ((gt2004CurrentIndex<(int)GT2004ParametersSet::numberOfParameters)&&(evolutionMode!=0))
    {
      //send the parameters to be measured to motion, just to be sure it has the correct ones
      gt2004Parameters = *param;
      walkParameterTimeStamp=SystemCall::getCurrentSystemTime();
    }
    if (gt2004CurrentIndex<(int)GT2004ParametersSet::numberOfParameters)
    {
      sprintf(txt," (%3.1f, %3.1f, %1.3f), corrected [%3.1f, %3.1f, %1.3f]:",motionRequest.walkRequest.walkParams.translation.x,motionRequest.walkRequest.walkParams.translation.y,motionRequest.walkRequest.walkParams.rotation,param->correctedMotion.translation.x,param->correctedMotion.translation.y,param->correctedMotion.rotation);
    }
    OUTPUT(idText,text,"measure " << GT2004ParametersSet::getIndexString(gt2004CurrentIndex) << txt);
    //send them as debug message too to notify GT2004ParametersDialog
    OUTPUT(idGT2004Parameters,bin,((gt2004CurrentIndex<GT2004ParametersSet::numberOfParameters)?*gt2004ParametersSet.getParameters(gt2004CurrentIndex):gt2004ExtraParameters));
    
    x0=y0=r0=t0=0;
    lastRot=-1000;
    lastTime=0;
    firstNullTime=0;
    lastNullTime=0;
    nullTime=0;
    speed=Pose2D(50*measurementRequest.rotation,0,0);
    clusterNumber=0;
    dr=dx=dy=0;
    goodFrames=0;
  }
  lastFrameNumber=robotPose.frameNumber;
  
  //start measuring after 2.0s:
  if (robotPose.frameNumber-startFrameNumber>250)
  {
    double actTime=0.008*robotPose.frameNumber;
    if ((robotPose.getValidity()>0.5)&&(fabs(robotPose.rotation)<1.25))
    {
      if (goodFrames++ >0)
      {
        //OUTPUT(idText,text,"actTime=" << actTime << ", rotDiff=" << robotPose.rotation-lastRot << ", rotSinceLast0=" << fabs(speed.rotation*(actTime-lastNullTime)-robotPose.rotation));
        if (
          ((speed.rotation<0)&&(robotPose.rotation-lastRot>0.015)&&(actTime-lastTime>0.2))||
          ((speed.rotation>0)&&(robotPose.rotation-lastRot<-0.015)&&(actTime-lastTime>0.2))||
          ((lastNullTime!=0)&&(fabs(speed.rotation*(actTime-lastNullTime)-robotPose.rotation)>3.2*pi)&&(actTime-lastTime>1.0))||
          ((lastNullTime==0)&&(actTime-lastTime>1.0))
          )
        {
          //a new cluster starts, because measured value is too far away from expected value
          
          lastNullTime=nullTime;
          if (firstNullTime==0)
          {
            firstNullTime=lastNullTime;
          }
          else
          {
            if (clusterFrames>2)
            {
              //last cluster finished: calc resulting speed:
              double rotDiff=(nullTime-firstNullTime)*speed.rotation/2/pi-(clusterNumber-1);
              if (rotDiff>1)
              {
                //according to last speed we have to be an rotation further,
                //so we missed a cluster. correct that:
                clusterNumber++;
                OUTPUT(idText,text,"!!!rotDiff=" << rotDiff <<" -> clusterNumber++");
              }
              speed.rotation=(speed.rotation>0)?
                2*pi*(clusterNumber-1)/(nullTime-firstNullTime):
              -2*pi*(clusterNumber-1)/(nullTime-firstNullTime);
              sprintf(txt,"cluster=%i, goodFrames=%i, clusterSpeed=%.3f, nullTime=%.2f, rotDiff=%.3f, speed=%.2f",clusterNumber,goodFrames,dr,nullTime,rotDiff,speed.rotation);
              OUTPUT(idText,text, txt);
            }
            if ((clusterFrames>10)&&(headControlMode.headControlMode==HeadControlMode::watchOrigin))
            {
              //incorporate old dx/dy into speed, if it was a 'valid' cluster
              speed.translation *= (clusterNumber-2);
              speed.translation.x += dx;
              speed.translation.y += dy;
              speed.translation /= (clusterNumber-1);
              sprintf(txt,"dxcluster=%3.1f, dycluster=%3.1f, dx=%3.1f, dy=%3.1f",dx,dy,speed.translation.x,speed.translation.y);
              OUTPUT(idText,text, txt);
            }
          }
          
          clusterNumber++;
          //OUTPUT(idText,text, "starting cluster " << clusterNumber);
          clusterFrames=0;


          t0=actTime;
          //we have to use COG/point of turn for x/y here:
          x0=robotPose.translation.x-100*cos(robotPose.rotation);
          y0=robotPose.translation.y-100*sin(robotPose.rotation);
          r0=robotPose.rotation;
        }
        else
        {
          clusterFrames++;
          //we are still in the same cluster
          double t=actTime-t0;
          //we have to use COG/point of turn for x/y here:
          double xt=robotPose.translation.x-100*cos(robotPose.rotation);
          double yt=robotPose.translation.y-100*sin(robotPose.rotation);
          double rt=robotPose.rotation;
          
          dr=(rt-r0)/t;
          
          if (t>0.2)
          {
            //only measure dx, dy if we are continously localized at least 0.2 seconds
            
            if (fabs(dr)<0.001)
            {
              //this should never happen because
              //measure blind is only useful for dr!=0
              dr=0.001;
            }
            //Step4.1 (if dr!=0):
            double cdrt2=cos(r0+dr*t/2);
            double sdrt2=sin(r0+dr*t/2);
            dx=dr/2/sin(dr*t/2)*((xt-x0)*cdrt2+(yt-y0)*sdrt2);
            dy=dr*sin(dr*t/2)*((y0-yt)*cdrt2+(xt-x0)*sdrt2)/(cos(dr*t)-1);
          }
          //CARE: this is not position change of neck but of COG!
          
          nullTime=(r0*dr>0)?t0-fabs(r0/dr):t0+fabs(r0/dr);
          //sprintf(txt,"t0=%.3f, actTime=%.3f, r0=%.3f, rt=%.3f, t=%.3f, dr=%.3f, nullTime=%.2f",t0,actTime,r0,rt,t,dr,nullTime);
          //OUTPUT(idText,text, txt);
          
          if (clusterNumber<=1)
          {
            speed.rotation=dr;
          }
          else if (clusterNumber==2)
          {
            double rotDiff=(nullTime-firstNullTime)*speed.rotation/2/pi-(clusterNumber-1);
            if ((rotDiff>1.5)&&(goodFrames>3))
            {
              //according to last speed we have to be at least an rotation further,
              //so we missed a cluster. correct that:
              clusterNumber++;
              OUTPUT(idText,text,"!!!rotDiff=" << rotDiff <<" -> clusterNumber++");
            }
            speed.rotation=(speed.rotation>0)?
              2*pi*(clusterNumber-1)/(nullTime-firstNullTime):
            -2*pi*(clusterNumber-1)/(nullTime-firstNullTime);
            if ((headControlMode.headControlMode==HeadControlMode::watchOrigin))
            {
              speed.translation.x=dx;
              speed.translation.y=dy;
            }
            //sprintf(txt,"cluster=%i, clusterSpeed=%.3f, nullTime=%.2f, rotDiff=%.3f, dx=%.1f, dy=%.1f, rspeed=%.3f",clusterNumber,dr,nullTime,rotDiff,speed.translation.x,speed.translation.y,speed.rotation);
            //OUTPUT(idText,text, txt);
          }
        }
        lastRot=robotPose.rotation;
        lastTime=actTime;
      }
    }
    else
    {
      goodFrames=0;
    }
    gt2004ParametersCalibration[gt2004CurrentIndex]=speed;
  }
}


void GT2004BasicBehaviorSendCurrentGT2004ParametersAndChooseNext::execute()
{
  char txt[512];
  sprintf(txt,"  dx=%3.1f, dy=%3.1f, dr=%1.3f",gt2004ParametersCalibration[gt2004CurrentIndex].translation.x, gt2004ParametersCalibration[gt2004CurrentIndex].translation.y, gt2004ParametersCalibration[gt2004CurrentIndex].rotation);
  
  if (GT2004ParametersSet::getSpeed(gt2004ParametersCalibration[gt2004CurrentIndex])>=2)
  {
    gt2004ParametersCalibration[gt2004CurrentIndex]=Pose2D(0,0,0);
    return;
  }

  if ((gt2004CurrentIndex<(int)GT2004ParametersSet::numberOfParameters)&&(evolutionMode!=0))
  {
    //initialize rand(), but only if we dont want direct comparisons anymore and really use evolution here
    //its good to do this here, because in constructor can always be the same time...
    srand(SystemCall::getCurrentSystemTime());

    if (((measurementRequest.translation.abs()==0)||(gt2004ParametersCalibration[gt2004CurrentIndex].translation.abs()>0))&&
        ((fabs(measurementRequest.rotation)<0.5)||(fabs(gt2004ParametersCalibration[gt2004CurrentIndex].rotation)>0.02))
       )
    {
      if (!gt2004ParametersSet.isMaxSpeedIndex(gt2004CurrentIndex))
      {
        //correct parameters
        if (gt2004ParametersSet.getParameters(gt2004CurrentIndex)->reportRealMotion(gt2004ParametersCalibration[gt2004CurrentIndex]))
        {
          Pose2D corrected=gt2004ParametersSet.getParameters(gt2004CurrentIndex)->correctedMotion;
          sprintf(&txt[strlen(txt)],"    -> corrected [%3.1f, %3.1f, %1.3f]",corrected.translation.x, corrected.translation.y, corrected.rotation);
        }
      }
      else
      {
        //2do: make max speed stuff better
      }
    
      //mirror the parameters
      gt2004ParametersSet.mirrorThis(gt2004CurrentIndex);
      int mirror=gt2004ParametersSet.getIndexOfMirror(gt2004CurrentIndex);
      if (mirror>=0)
      {
        gt2004ParametersCalibration[mirror]=gt2004ParametersCalibration[gt2004CurrentIndex];
        gt2004ParametersCalibration[mirror].translation.y = -gt2004ParametersCalibration[mirror].translation.y;
        gt2004ParametersCalibration[mirror].rotation = -gt2004ParametersCalibration[mirror].rotation;
      }
      //2do: the mirror is not sent to Motion
    }
    
    //send the corrected parameters to motion
    gt2004Parameters = *gt2004ParametersSet.getParameters(gt2004CurrentIndex);
    walkParameterTimeStamp=SystemCall::getCurrentSystemTime();
    
    //send this as debug message too to notify GT2004ParametersDialog
    OUTPUT(idGT2004Parameters,bin,gt2004Parameters);
    
    //2do: while loop. repeat until all parameters are good enough
    
    //we only need one of the mirrored partners (always the one with the higher index = lturn)
    //and no stand here, therefore we only have 66 cases (+default):
    switch ((GT2004ParametersSet::IndexName)gt2004CurrentIndex)
    {
    case GT2004ParametersSet::no_turn_0_fast:        gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min180_fast; break;
    case GT2004ParametersSet::no_turn_min180_fast:   gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_0_med; break;
    case GT2004ParametersSet::no_turn_0_med:         gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min180_med; break;
    case GT2004ParametersSet::no_turn_min180_med:    gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_0_slow; break;
    case GT2004ParametersSet::no_turn_0_slow:        gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min180_slow; break;
    case GT2004ParametersSet::no_turn_min180_slow:   gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min45_fast; break;
      
    case GT2004ParametersSet::no_turn_min45_fast:    gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min135_fast; break;
    case GT2004ParametersSet::no_turn_min135_fast:   gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min45_med; break;
    case GT2004ParametersSet::no_turn_min45_med:     gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min135_med; break;
    case GT2004ParametersSet::no_turn_min135_med:    gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min45_slow; break;
    case GT2004ParametersSet::no_turn_min45_slow:    gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min135_slow; break;
    case GT2004ParametersSet::no_turn_min135_slow:   gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min90_fast; break;
    case GT2004ParametersSet::no_turn_min90_fast:    gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min90_med; break;
    case GT2004ParametersSet::no_turn_min90_med:     gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_min90_slow; break;
    case GT2004ParametersSet::no_turn_min90_slow:    gt2004CurrentIndex=(int)GT2004ParametersSet::much_lturn_slow; break;
      
    case GT2004ParametersSet::much_lturn_slow:       gt2004CurrentIndex=(int)GT2004ParametersSet::much_lturn_med; break;
    case GT2004ParametersSet::much_lturn_med:        gt2004CurrentIndex=(int)GT2004ParametersSet::much_lturn_fast; break;
    case GT2004ParametersSet::much_lturn_fast:       gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_0_fast; break;
      
    case GT2004ParametersSet::few_lturn_0_fast:      gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min180_fast; break;
    case GT2004ParametersSet::few_lturn_min180_fast: gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_45_fast; break;
    case GT2004ParametersSet::few_lturn_45_fast:     gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min135_fast; break;
    case GT2004ParametersSet::few_lturn_min135_fast: gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_90_fast; break;
    case GT2004ParametersSet::few_lturn_90_fast:     gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min90_fast; break;
    case GT2004ParametersSet::few_lturn_min90_fast:  gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_135_fast; break;
    case GT2004ParametersSet::few_lturn_135_fast:    gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min45_fast; break;
    case GT2004ParametersSet::few_lturn_min45_fast:  gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_0_med; break;
      
    case GT2004ParametersSet::few_lturn_0_med:       gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min180_med; break;
    case GT2004ParametersSet::few_lturn_min180_med:  gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_45_med; break;
    case GT2004ParametersSet::few_lturn_45_med:      gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min135_med; break;
    case GT2004ParametersSet::few_lturn_min135_med:  gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_90_med; break;
    case GT2004ParametersSet::few_lturn_90_med:      gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min90_med; break;
    case GT2004ParametersSet::few_lturn_min90_med:   gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_135_med; break;
    case GT2004ParametersSet::few_lturn_135_med:     gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min45_med; break;
    case GT2004ParametersSet::few_lturn_min45_med:   gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_0_slow; break;
      
    case GT2004ParametersSet::few_lturn_0_slow:      gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min180_slow; break;
    case GT2004ParametersSet::few_lturn_min180_slow: gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_45_slow; break;
    case GT2004ParametersSet::few_lturn_45_slow:     gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min135_slow; break;
    case GT2004ParametersSet::few_lturn_min135_slow: gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_90_slow; break;
    case GT2004ParametersSet::few_lturn_90_slow:     gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min90_slow; break;
    case GT2004ParametersSet::few_lturn_min90_slow:  gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_135_slow; break;
    case GT2004ParametersSet::few_lturn_135_slow:    gt2004CurrentIndex=(int)GT2004ParametersSet::few_lturn_min45_slow; break;
    case GT2004ParametersSet::few_lturn_min45_slow:  gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_0_fast; break;
      
    case GT2004ParametersSet::med_lturn_0_fast:      gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min180_fast; break;
    case GT2004ParametersSet::med_lturn_min180_fast: gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_45_fast; break;
    case GT2004ParametersSet::med_lturn_45_fast:     gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min135_fast; break;
    case GT2004ParametersSet::med_lturn_min135_fast: gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_90_fast; break;
    case GT2004ParametersSet::med_lturn_90_fast:     gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min90_fast; break;
    case GT2004ParametersSet::med_lturn_min90_fast:  gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_135_fast; break;
    case GT2004ParametersSet::med_lturn_135_fast:    gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min45_fast; break;
    case GT2004ParametersSet::med_lturn_min45_fast:  gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_0_med; break;
      
    case GT2004ParametersSet::med_lturn_0_med:       gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min180_med; break;
    case GT2004ParametersSet::med_lturn_min180_med:  gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_45_med; break;
    case GT2004ParametersSet::med_lturn_45_med:      gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min135_med; break;
    case GT2004ParametersSet::med_lturn_min135_med:  gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_90_med; break;
    case GT2004ParametersSet::med_lturn_90_med:      gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min90_med; break;
    case GT2004ParametersSet::med_lturn_min90_med:   gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_135_med; break;
    case GT2004ParametersSet::med_lturn_135_med:     gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min45_med; break;
    case GT2004ParametersSet::med_lturn_min45_med:   gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_0_slow; break;
      
    case GT2004ParametersSet::med_lturn_0_slow:      gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min180_slow; break;
    case GT2004ParametersSet::med_lturn_min180_slow: gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_45_slow; break;
    case GT2004ParametersSet::med_lturn_45_slow:     gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min135_slow; break;
    case GT2004ParametersSet::med_lturn_min135_slow: gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_90_slow; break;
    case GT2004ParametersSet::med_lturn_90_slow:     gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min90_slow; break;
    case GT2004ParametersSet::med_lturn_min90_slow:  gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_135_slow; break;
    case GT2004ParametersSet::med_lturn_135_slow:    gt2004CurrentIndex=(int)GT2004ParametersSet::med_lturn_min45_slow; break;
    case GT2004ParametersSet::med_lturn_min45_slow:
    default:
      gt2004CurrentIndex=(int)GT2004ParametersSet::no_turn_0_fast;
    }
    measurementRequest=gt2004ParametersSet.getParameters(gt2004CurrentIndex)->requestedMotion;
  }
  OUTPUT(idText,text,txt);
}

void GT2004BasicBehaviorNextGT2004ParametersToBeMeasured::execute()
{
  char txt[512];
  sprintf(txt,"  dx=%3.1f, dy=%3.1f, dr=%1.3f",gt2004ParametersCalibration[gt2004CurrentIndex].translation.x, gt2004ParametersCalibration[gt2004CurrentIndex].translation.y, gt2004ParametersCalibration[gt2004CurrentIndex].rotation);
  OUTPUT(idText,text,txt);
  
  double speed=GT2004ParametersSet::getSpeed(measurementRequest);
  double direction=GT2004ParametersSet::getDirection(measurementRequest);
  double ratio=GT2004ParametersSet::getRatio(measurementRequest);

  double mSpeed=GT2004ParametersSet::getSpeed(gt2004ParametersCalibration[gt2004CurrentIndex]);
  
  trials++;
  if ((speed>0.001)&&(mSpeed<2)&&((mSpeed>0.001)||(trials>2)))
  {
    requestTable[tableIndex]=measurementRequest;
    measureTable[tableIndex]=gt2004ParametersCalibration[gt2004CurrentIndex];
    //calc soll/ist ratio and (re)set maxima:
    double speedRatio,yRatio,rRatio;
    if ((speed<0.4)||(gt2004ParametersCalibration[gt2004CurrentIndex].translation.abs()==0))
    {
      speedRatio=1;
      maxSpeed=0;
      maxSpeedRatio=0;
    }
    else
    {
      speedRatio=mSpeed/speed;
      if (mSpeed>maxSpeed)
      {
        maxSpeed=mSpeed;
      }
      if (speedRatio>maxSpeedRatio)
      {
        maxSpeedRatio=speedRatio;
      }
    }
    if ((fabs(measurementRequest.translation.y)<100)||(gt2004ParametersCalibration[gt2004CurrentIndex].translation.y==0))
    {
      yRatio=1;
      maxYRatio=0;
    }
    else
    {
      yRatio=gt2004ParametersCalibration[gt2004CurrentIndex].translation.y/measurementRequest.translation.y;
      if (yRatio>maxYRatio)
      {
        maxYRatio=yRatio;
      }
    }
    if (fabs(measurementRequest.rotation)<0.25)
    {
      rRatio=1;
      maxRRatio=0;
    }
    else
    {
      rRatio=gt2004ParametersCalibration[gt2004CurrentIndex].rotation/measurementRequest.rotation;
      if (rRatio>maxRRatio)
      {
        maxRRatio=rRatio;
      }
      if (fabs(measurementRequest.rotation)>2)
      {
        //this is to abort fast turning earlier (when rmess/rreq < 0.7(rmess/rreq)_max)
        speedRatio=rRatio;
        maxSpeedRatio=maxRRatio;
      }
    }
    
    trials=0;
    if ((tableIndex==0)||((mSpeed>0.92*maxSpeed)&&(mSpeed>maxSpeed-0.1)&&(speedRatio>0.7*maxSpeedRatio)&&(yRatio>0.66*maxYRatio)&&(rRatio>0.5*maxRRatio)&&(tableIndex<39)))
    {
      tableIndex++;
      GT2004ParametersSet::setSpeed(measurementRequest,speed+0.05);
    }
    else
    {
      {
        OutTextFile file("measure.csv",true);
        file << gt2004ExtraParameters;
      }
      OutTextRawFile file("measure.csv",true);
      file << "\nRatio=" << ratio << ", Direction=" << (int)(direction*180.01/pi) <<"\n";
      file << "request_x, request_y, request_r, measured_x, measured_y, measured_r, speed_request, speed_measured, speed_ratio\n";
      file << "0.0, 0.0, 0.0, 0.0, 0.0, 0.0\n";
      for (int i=0; i<=tableIndex; i++)
      {
        double speedReq=GT2004ParametersSet::getSpeed(requestTable[i]);
        double speedMes=GT2004ParametersSet::getSpeed(measureTable[i]);
        sprintf(txt,"%3.1f, %3.1f, %1.3f, %3.1f, %3.1f, %1.3f, %1.3f, %1.3f, %1.3f\n",
                requestTable[i].translation.x, requestTable[i].translation.y, requestTable[i].rotation,
                measureTable[i].translation.x, measureTable[i].translation.y, measureTable[i].rotation,
                speedReq,speedMes,speedMes/speedReq);
        file << txt;
      }
      file << "\n";
      OUTPUT(idText,text,"last series appended to measure.csv");

      //only clear last measurement here (=when changing ratio or direction), because otherwise adjustment ist useful:
      gt2004ParametersCalibration[gt2004CurrentIndex]=Pose2D(0,0,0);
      tableIndex=0;
      maxSpeed=0;
      maxSpeedRatio=0; //care! speed/speed ratio here!
      
      GT2004ParametersSet::setSpeed(measurementRequest,0.05);
      if (fabs(ratio)<0.5)
      {
        GT2004ParametersSet::setDirection(measurementRequest,direction+pi_4);
      }
      if (fabs(GT2004ParametersSet::getDirection(measurementRequest))<0.1)
      {
        if (ratio<-0.5)
        {
          GT2004ParametersSet::setRatio(measurementRequest,-0.3);
        }
        else if (ratio<-0.2)
        {
          GT2004ParametersSet::setRatio(measurementRequest,-0.1);
        }
        else if (ratio<-0.05)
        {
          GT2004ParametersSet::setRatio(measurementRequest,0.0);
        }
        else if (ratio<0.05)
        {
          GT2004ParametersSet::setRatio(measurementRequest,0.1);
        }
        else if (ratio<0.2)
        {
          GT2004ParametersSet::setRatio(measurementRequest,0.3);
        }
        else if (ratio<0.5)
        {
          GT2004ParametersSet::setRatio(measurementRequest,1.0);
        }
        else
        {
          GT2004ParametersSet::setRatio(measurementRequest,-1.0);
        }
      }
      unsigned int sec=SystemCall::getCurrentSystemTime()/1000;
      OUTPUT(idText,text,"Starting Ratio="<<GT2004ParametersSet::getRatio(measurementRequest)<<", Direction="<<(int)(GT2004ParametersSet::getDirection(measurementRequest)*180.01/pi)<<", Time=" <<(sec/60)<<":"<<(sec%60));
    }
    gt2004ExtraParameters.requestedMotion=measurementRequest;
    //send this as debug message to notify GT2004ParametersDialog
    OUTPUT(idGT2004Parameters,bin,gt2004ExtraParameters);
  }
}

/*
* Change Log
* 
* $Log: GT2004EvolutionBasicBehaviors.cpp,v $
* Revision 1.7  2004/07/10 00:13:53  spranger
* renaming for coderelease and preparations for gt2005
*
* Revision 1.6  2004/07/07 15:13:00  dueffert
* evolution uses two populations now
*
* Revision 1.5  2004/06/20 18:18:46  dueffert
* goTo(EvolutionStart)Point improved
*
* Revision 1.4  2004/06/14 16:54:56  juengel
* Removed some WalkingEngineParameterSets.
*
* Revision 1.3  2004/06/07 10:16:50  dueffert
* bug fixed
*
* Revision 1.2  2004/06/02 17:18:23  spranger
* MotionRequest cleanup
*
* Revision 1.1  2004/05/29 18:18:19  dueffert
* walk parameter evolution, measurement and calibration stuff ported to GT2004_WM
*
* Revision 1.1  2004/05/27 13:31:26  dueffert
* walking evolution stuff separated
*
* Revision 1.43  2004/05/24 12:48:26  dueffert
* outputs corrected
*
* Revision 1.42  2004/05/23 13:31:10  dueffert
* output improved; mirroring corrected
*
* Revision 1.41  2004/05/20 17:18:35  dueffert
* automatic measurement (hopefully) finalized
*
* Revision 1.40  2004/05/19 07:59:44  dueffert
* automatic walk speed measurement significantly improved
*
* Revision 1.39  2004/05/17 11:16:51  dueffert
* blind measurement improved
*
* Revision 1.38  2004/05/14 22:53:07  roefer
* Warnings removed
*
* Revision 1.37  2004/05/14 10:15:49  dueffert
* exact measurement behavior with logging implemented
*
* Revision 1.36  2004/05/12 14:23:31  dueffert
* measurement basic behavior added
*
* Revision 1.35  2004/05/11 16:33:36  dueffert
* send current parameters even if they were not auto evolved/changed
*
* Revision 1.34  2004/05/04 10:48:58  loetzsch
* replaced all enums
* xxxBehaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
* by
* behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
* (this mechanism was neither fully implemented nor used)
*
* Revision 1.33  2004/05/03 18:52:38  dueffert
* comments corrected; max speed / rest distinction added
*
* Revision 1.32  2004/04/29 15:15:00  dueffert
* measurement is now frame instead of time based
*
* Revision 1.31  2004/04/19 10:34:55  dueffert
* parameters tuned to allow measurement oft *fast* walking
*
* Revision 1.30  2004/04/07 17:00:19  dueffert
* idea of srand() added
*
* Revision 1.29  2004/03/30 11:54:47  dueffert
* first useful evolutionMode implementation (0=none, 1=auto)
*
* Revision 1.28  2004/03/29 15:27:35  dueffert
* output improved, evolution mode idea added
*
* Revision 1.27  2004/03/26 09:24:17  dueffert
* missing conditions and one UDParameters for everything support added
*
* Revision 1.26  2004/03/24 15:09:39  dueffert
* do not let Motion calculate RobotPose updates during evolution runs
*
* Revision 1.25  2004/03/24 13:45:25  dueffert
* support for uniform noise mutation readded; evolution logic improved
*
* Revision 1.24  2004/03/19 16:37:27  dueffert
* blind measurement works for the first time
*
* Revision 1.23  2004/03/16 10:25:24  dueffert
* headControl for blind measurement changed
*
* Revision 1.22  2004/03/15 12:40:17  dueffert
* measurement of freely choosen MotionRequest allowed; output improved
*
* Revision 1.21  2004/03/12 17:23:36  dueffert
* own bug fixed
*
* Revision 1.20  2004/03/12 17:11:38  dueffert
* omni-evo uses parameters in package now; blind speed calculation improved
*
* Revision 1.19  2004/03/10 15:01:07  dueffert
* first simple fast turn (blind) measurement implemented
*
* Revision 1.18  2004/03/10 10:02:22  dueffert
* population size tweaked; statistics output added
*
* Revision 1.17  2004/03/09 11:40:55  wachter
* included Platform/GTAssert.h
*
* Revision 1.16  2004/03/09 08:46:30  dueffert
* second measurement behavior added
*
* Revision 1.15  2004/03/03 08:34:16  dueffert
* output improved
*
* Revision 1.14  2004/03/01 15:10:04  dueffert
* measurement order defined, measurement output beautified
*
* Revision 1.13  2004/02/29 17:28:22  dueffert
* UDParameters have char* names now
*
* Revision 1.12  2004/02/29 13:42:58  dueffert
* UDParametersSet symmetries handled
*
* Revision 1.11  2004/02/27 16:43:03  dueffert
* UDEvolutionRequest introduced
*
* Revision 1.10  2004/02/23 16:48:11  dueffert
* several improvements for measurement of walking
*
* Revision 1.9  2004/02/18 14:49:20  dueffert
* behavior control can now change walking parameters
*
* Revision 1.8  2004/02/10 11:56:53  dueffert
* typo corrected
*
* Revision 1.7  2004/02/10 11:19:19  dueffert
* simple basic behavior for evolution added, another improved, beautified
*
* Revision 1.6  2004/02/03 13:20:48  spranger
* renamed all references to  class BallPosition to BallModel (possibly changed include files)
*
* Revision 1.5  2003/12/09 16:29:06  jhoffman
* removed OUTPUTs
*
* Revision 1.4  2003/12/09 15:15:20  dueffert
* WalkAccelerationRestrictor replaced by PIDsmoothedValue for evolving
*
* Revision 1.3  2003/11/19 13:49:39  dueffert
* better positioning
*
* Revision 1.2  2003/10/28 13:27:22  dueffert
* spelling and evolution logic improved
*
* Revision 1.1  2003/10/22 22:18:45  loetzsch
* prepared the cloning of the GT2003BehaviorControl
*
* Revision 1.1  2003/10/06 13:39:31  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.18  2003/09/25 10:14:26  juengel
* Removed some walk-types.
*
* Revision 1.17  2003/09/24 15:29:20  dueffert
* first somehow working automatic walk parameter evolution
*
* Revision 1.16  2003/09/11 15:21:31  dueffert
* starting of evolution added
*
* Revision 1.15  2003/08/08 14:30:04  dueffert
* some evolution implementation added
*
* Revision 1.14  2003/08/04 07:46:39  roefer
* Challenge 2 modifications
*
* Revision 1.13  2003/07/30 14:52:03  dueffert
* walk evolution agent added
*
* Revision 1.12  2003/07/09 11:44:26  jhoffman
* obstacle avoidance simple behavior mode as used in challenge 3
* known bug: sometimes stops in front of obstacle (but eventually the situation is resolved)
*
* Revision 1.11  2003/07/08 06:11:56  dueffert
* further debug output removed
*
* Revision 1.10  2003/07/08 01:27:53  dueffert
* another debug output suppressed
*
* Revision 1.9  2003/07/08 01:21:00  dueffert
* debug output suppressed
*
* Revision 1.8  2003/07/07 07:15:36  jhoffman
* commented out "auto" mode for dribbling
*
* Revision 1.7  2003/07/07 07:08:19  jhoffman
* dribble as a basic behavior added to kicktest
*
* Revision 1.6  2003/07/06 20:34:30  dueffert
* GT2003BasicBehaviorGoToBallWithDirection added
*
* Revision 1.5  2003/07/05 13:35:49  jhoffman
* another bug fixed
*
* Revision 1.3  2003/07/05 13:32:46  jhoffman
* obstacle avoider challenge additions
* plus: kick-off behavior uses "simple-avoidance"
*
* Revision 1.2  2003/07/04 16:24:24  jhoffman
* updated obstacle avoider (simple behavior) to be used in challenge
*
* Revision 1.1.1.1  2003/07/02 09:40:23  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.9  2003/06/04 00:43:45  loetzsch
* put the GT2003WalkAccelerationRestrictor into two extra files in /Src/Tools
*
* Revision 1.8  2003/06/02 04:45:51  loetzsch
* "get-to-position-and-avoid-obstacles" now faster turns
*
* Revision 1.7  2003/05/14 13:08:38  risler
* removed DefaultObstaclesLocator
* renamed MicroSectorsObstaclesLocator to DefaultObstaclesLocator
* ObstaclesModel contains increased number of sectors
* DefaultObstaclesLocator clean up
*
* Revision 1.6  2003/05/09 15:31:18  loetzsch
* added time-after-which-communicated-ball-are-accepted
*
* Revision 1.5  2003/05/07 13:02:34  risler
* added first continuous basic behaviors to GT2003BehaviorControl
*
* Revision 1.4  2003/05/06 11:18:33  goehring
* Cast bug removed
*
* Revision 1.3  2003/05/06 07:59:19  dueffert
* generating XML symbols in C
*
* Revision 1.2  2003/05/05 19:22:03  risler
* added classes Simple/ContinuousBasicBehaviors for registering basic behaviors
*
* Revision 1.1  2003/05/03 15:14:02  loetzsch
* GT2003BehaviorControl first draft
*
*/

---