Modules/BehaviorControl/GT2005BehaviorControl/GT2005BasicBehaviors/GT2005WalkCalibrationBasicBehaviors.cpp

Go to the documentation of this file.

#include "Tools/Math/PIDsmoothedValue.h"
#include "GT2005WalkCalibrationBasicBehaviors.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Math/Matrix2x2.h"
#include "Tools/Math/Common.h"

#include "Tools/Debugging/DebugDrawings2.h"
#include "Tools/Debugging/Debugging.h"

GT2005WalkCalibrationMainBehavior::GT2005WalkCalibrationMainBehavior(BehaviorControlInterfaces& interfaces, Xabsl2ErrorHandler& errorHandler)
: Xabsl2BasicBehavior("walk-calibration", errorHandler),
BehaviorControlInterfaces(interfaces)

{
  registerParameter("walk-calibration.initialize", initializeParameter);
  registerParameter("walk-calibration.rotation", rotationParameter);

  result = resultUnready;
  state = stateNotInitialized;
  tempPoseCount = 0;
  startPoseCalculated = false;
}

void GT2005WalkCalibrationMainBehavior::removeExtreme(bool *pBadIndex, bool removeMaximum)
{
    int extremeIndex;
    double extremeSpeed;
    double tempSpeed;

    //repeat this 4 times (2 min, 2 max)
    extremeIndex = -1;
    
    if(removeMaximum)
      extremeSpeed = 0.0;
    else
      extremeSpeed = 1000000;


    for(int i = 0; i < tempPoseCount; i++)
    {
      if(!pBadIndex[i])
      {
        tempSpeed = pow(tempPose[i].translation.x, 2) + pow(tempPose[i].translation.y, 2);
        if(removeMaximum && tempSpeed > extremeSpeed)
        {
          extremeSpeed = tempSpeed;
          extremeIndex = i;
        }
        else if(!removeMaximum && tempSpeed < extremeSpeed)
        {
          extremeSpeed = tempSpeed;
          extremeIndex = i;
        }
      }
    }
    if(extremeIndex >= 0)
      pBadIndex[extremeIndex] = true;
}


void GT2005WalkCalibrationMainBehavior::execute()
{
  if(initializeParameter)
    Initialize();
  else
  {
    bool performMeasure = true;

#ifdef _WIN32  
    camRobotPose = robotPose;
    camTimestamp = SystemCall::getCurrentSystemTime();
    robotNotDetectedCount = 0;

#else
    camRobotPose = gtCamWorldState.getRobotPose(robotNumber, robotColor);
    camTimestamp = gtCamWorldState.getTimestamp();
    if((camRobotPose.translation.x == 0.0) &&
      (camRobotPose.translation.y == 0.0) && 
      (camRobotPose.rotation == 0.0))
      robotNotDetectedCount++;
    else
      robotNotDetectedCount = 0;
#endif

    // robot not detected by gtCam
    if(robotNotDetectedCount > 0)
    {
      state = stateMeasureStart;      
      result = (robotNotDetectedCount >= 10) ? resultInteraction : resultReposition;
      performMeasure = false;
      OUTPUT(idText, text, "GTWalkCalibration: Robot not recognized or worldstate missing (" << robotNotDetectedCount << ". times)");
    }

    // dog is outside camera range or field
    Vector2<double> temp = camRobotPose.translation;
    if(fabs(temp.y) > 1900 || temp.x > 2400 || temp.x < 0)
    {
      result = resultReposition;
      state = stateMeasureStart;
      startPoseBadCount++;
      performMeasure = false;
      OUTPUT(idText, text, "GTWalkCalibration: Robot out of measure arrea (" << startPoseBadCount << ". times)");
    }

    if(performMeasure)
      Measure();

    //catch too many bad start positions in a row
    if(startPoseBadCount >= 5)
    {
      OUTPUT(idText, text, "start pose was bad " << startPoseBadCount << " times in a row, requesting manual interaction");
      state = stateMeasureStart;
      result = resultInteraction;
    }

    //calculation of start position
    if(result == resultReposition)
    {
      if(tempPoseCount > 0)
      {
        Pose2D mid;

        for(int i=0;i< tempPoseCount; i++)
        {
          mid.translation.x += tempPose[i].translation.x;
          mid.translation.y += tempPose[i].translation.y;
          mid.rotation += tempPose[i].rotation;
        }
        mid.translation.x /= (double)tempPoseCount;
        mid.translation.y /= (double)tempPoseCount;
        mid.rotation /= (double)tempPoseCount;

        startPose = calculateStartPose(mid);
        startPoseCalculated = true;
        //TODO also repeat on line change??
      }
      else 
      {
        startPose = calculateStartPose(measurePoints[currentPoint].controlled);
        startPoseCalculated = false;
      }
      startPoseAngle = toDegrees(startPose.rotation);
    }
  }
}

void GT2005WalkCalibrationMainBehavior::Initialize()
{
  OUTPUT(idText, text, "GTWalkCalibration: Initializing");
  state = stateMeasureStart;
  measureStatus = before;
  robotColor = getPlayer().getTeamColor();
  robotNumber = getPlayer().getPlayerNumber();
  currentTable = 0;
  currentPoint = 0;
  currentRotation = 0;
  startOffset = 2000.0;
  measureDuration = 4500.0; //4500.0 for normal plane calibartion 9500.0 for rotation only calibration
  stopOffset = 500.0;

  /*measurePoints[0].controlled = Pose2D(-1.6666 * pi, 0, 0);
  measurePoints[1].controlled = Pose2D(-1.3000 * pi, 0, 0);
  measurePoints[2].controlled = Pose2D(-1.0000 * pi, 0, 0);
  measurePoints[3].controlled = Pose2D(-0.8333 * pi, 0, 0);
  measurePoints[4].controlled = Pose2D(-0.6400 * pi, 0, 0);
  measurePoints[5].controlled = Pose2D(-0.4000 * pi, 0, 0);
  measurePoints[6].controlled = Pose2D(-0.3000 * pi, 0, 0);
  measurePoints[7].controlled = Pose2D(-0.2000 * pi, 0, 0);
  measurePoints[8].controlled = Pose2D(-0.1500 * pi, 0, 0);
  measurePoints[9].controlled = Pose2D(-0.1000 * pi, 0, 0);
  measurePoints[10].controlled = Pose2D(0.0000 * pi, 0, 0);
  measurePoints[11].controlled = Pose2D(0.1000 * pi, 0, 0);
  measurePoints[12].controlled = Pose2D(0.1500 * pi, 0, 0);
  measurePoints[13].controlled = Pose2D(0.2000 * pi, 0, 0);
  measurePoints[14].controlled = Pose2D(0.3000 * pi, 0, 0);
  measurePoints[15].controlled = Pose2D(0.4000 * pi, 0, 0);
  measurePoints[16].controlled = Pose2D(0.6400 * pi, 0, 0);
  measurePoints[17].controlled = Pose2D(0.8333 * pi, 0, 0);
  measurePoints[18].controlled = Pose2D(1.0000 * pi, 0, 0);
  measurePoints[19].controlled = Pose2D(1.3000 * pi, 0, 0);
  measurePoints[20].controlled = Pose2D(1.6666 * pi, 0, 0);//*/


  double measurePointsX[] = {-300, -250, -200, -150, -125, -100, -50, 0, 50, 100, 125, 150, 200, 250, 300};
  double measurePointsY[] = {-300, -250, -200, -150, -100, -50, 0, 50, 100, 150, 200, 250, 300};
  for(int i = 0; i < 15; i++)
  {
    for(int j = 0; j < 13; j++)
    {
      measurePoints[i * 13 + j].controlled = Pose2D(rotationParameter / 180.0 * pi, measurePointsX[i], measurePointsY[j]); //0.87266
    }
  }

  InBinaryFile file("_Measure.bin");
  if(file.exists())
  {
    Pose2D controlled;
    for(int i = 0; i < measureSize; i++)
    {
      if(file.eof())
        break;

      file >> controlled;

      if(controlled != measurePoints[i].controlled)
      {
        OUTPUT(idText, text, "controlled Points mismatch, cannot resume ");
        currentPoint = 0;
        break;
      }

      if(file.eof())
        break;

      file >> measurePoints[i].measured;
      currentPoint = i + 1;
    }
    if(currentPoint > 0)
      OUTPUT(idText, text, "GTWalkCalibration: resuming at measure #" << currentPoint);

    sprintf(filename, "od_%i%s.dat", (int)fabs(rotationParameter), rotationParameter < 0 ? "m" : "");
  }

  center.x = 1200.0;
  center.y = 0.0;

  startPoseBadCount = 0;
  measureCount = 0;
  initializationTime = SystemCall::getCurrentSystemTime();
  result = resultReady;
}



void GT2005WalkCalibrationMainBehavior::Measure()
{
  switch(state)
  {
  case stateMeasureStart:
    {
      motionRequest.motionType = MotionRequest::walk;
      motionRequest.walkRequest.walkType = WalkRequest::normal;
      motionRequest.walkRequest.walkParams = Pose2D(0, 0, 0);

      measureStartTime = camTimestamp + startOffset;
      measureStopTime = measureStartTime + measureDuration;
      rotationTemp = 0;
      tempPoseCount = 0;

      double timePerMeasure, timeLeft;
      if(measureCount > 0)
        timePerMeasure = (SystemCall::getCurrentSystemTime() - initializationTime) / ((double)measureCount);
      else
        timePerMeasure = 0.0;

      timeLeft = (measureSize - currentPoint) * timePerMeasure;
      OUTPUT(idText, text, "PWR: " << SystemCall::getRemainingPower() << "%, ETA: " << timeLeft / 1000.0 / 60.0 << "min");

      OUTPUT(idText, text, "GTWalkCalibration: starting measure #" << currentPoint);
      state = stateMeasure;
      measureStatus = before;
      result = resultMeasure;

      //clear qSum
      qSum = 0.0;
      qCount = 0;

      break;
    }
  case stateMeasure:
    {
      Pose2D request;
      
      motionRequest.motionType = MotionRequest::walk;
      motionRequest.walkRequest.walkType = WalkRequest::normal; 
      request = measurePoints[currentPoint].controlled;
      //hack for calibration
      request.translation.x /= 1.5; // 0..450 -> 0..300
      request.translation.y /= 1.5; // 0..450 -> 0..300
      request.rotation *= 1.5; // 0..200 -> 0..300 
      motionRequest.walkRequest.walkParams = request;

      if(measureStatus == measuring)
      {
        double qRms, qDist;

        //stdaweichung abstand zur kamera updaten
        qCount++;
        qDist = (center - camRobotPose.translation).abs();
        qSum += qDist * qDist;
        qRms = sqrt(qSum/(double)qCount);

        //if we got the startpose from controlled values
        //and qRms is too high and we have a measueMent yet
        if(!startPoseCalculated && qRms > 800.0 && tempPoseCount > 0)
        {
          result = resultReposition;
          state = stateMeasureStart;
          startPoseBadCount++;
          OUTPUT(idText, text, "start positioning was too bad(" << startPoseBadCount << "), repeating measure");
          break;
        }
        //if qRms is far too high
        if(qRms > 1000.0)
        {
          result = resultReposition;
          state = stateMeasureStart;
          startPoseBadCount++;
          OUTPUT(idText, text, "we are too far off(" << startPoseBadCount <<") !!");
          break;
        }
      }

      if(camTimestamp >= measureStartTime && measureStatus == before)
      {
        translationStartPoint = camRobotPose.translation;
        rotationStartAngle = camRobotPose.rotation;
        tempTimeStart = measureStartTime = camTimestamp;

        tempTime = measureStartTime + 400;
        measureStatus = measuring;

        // vorsichtshalber auch mal komplett messen
        measureStartTime0 = measureStartTime;
        translationStartPoint0 = translationStartPoint;
        rotationStartAngle0 = rotationStartAngle;
      }
      else if(camTimestamp >= tempTime && measureStatus == measuring)
      {
        translationStopPoint = camRobotPose.translation;
        rotationStopAngle = camRobotPose.rotation;
        tempTimeStop = camTimestamp;

        if(tempTimeStart == tempTimeStop)
        {
          OUTPUT(idText, text, "GTWalkCalibration: measureStartTime = measureStopTime");
          state = stateMeasureStart;
          result = resultInteraction;
          break;
        }

        double angle = rotationStartAngle; //(rotationStopAngle + rotationStartAngle) / 2;
        Vector2<double> robotx, roboty;
        double a, b;
        robotx.x = cos(angle);
        robotx.y = sin(angle);
        roboty.x = -robotx.y;
        roboty.y = robotx.x;
        a = ((translationStopPoint.x - translationStartPoint.x) * roboty.y
          + (translationStartPoint.y - translationStopPoint.y) * roboty.x)
          / (robotx.x * roboty.y - robotx.y * roboty.x);
        b = (translationStopPoint.y - translationStartPoint.y - a * robotx.y) / roboty.y;
        tempPose[tempPoseCount].translation.x = a / ((tempTimeStop - tempTimeStart) / 1000);
        tempPose[tempPoseCount].translation.y = b / ((tempTimeStop - tempTimeStart) / 1000);
        tempPose[tempPoseCount].rotation = normalize(rotationStopAngle - rotationStartAngle) / ((tempTimeStop - tempTimeStart) / 1000);

        OUTPUT(idText, text, "GTWalkCalibration: rot(" << tempPoseCount << "): " << tempPose[tempPoseCount].rotation);

        // set new startPointParameters
        translationStartPoint = camRobotPose.translation;
        rotationStartAngle = camRobotPose.rotation;
        tempTimeStart = camTimestamp;
        tempTime = tempTimeStart + 400;

        // dann wolln mer mal nen neues Häppchen messen
        if(tempPoseCount < 49) tempPoseCount++;
      }
      if(camTimestamp >= measureStopTime && measureStatus == measuring)
      {
        // vorsichtshalber auch mal komplett messen
        measureStopTime0 = camTimestamp;
        translationStopPoint0 = camRobotPose.translation;
        rotationStopAngle0 = camRobotPose.rotation;

        state = stateMeasureFinished;
        result = resultMeasure;
        measureStatus = end;
        break;
      }
      break;
    }
  case stateMeasureFinished:
    {
      bool badIndex[measureSize];

      OUTPUT(idText, text, " ");
      OUTPUT(idText, text, "GTWalkCalibration: finished measure #" << currentPoint);

      for(int i = 0 ;i < measureSize; i++)
        badIndex[i] = false;

      removeExtreme(badIndex, true);
      removeExtreme(badIndex, true);
      removeExtreme(badIndex, false);
      removeExtreme(badIndex, false);

      measurePoints[currentPoint].measured = Pose2D(0, 0, 0);
      int dieNackteWahrheit = 0;
      for(int i = 0; i < tempPoseCount; i++)
      {
        if (badIndex[i])
        {
          OUTPUT(idText, text, "discarded index " << i);
        }
        else
        {
          measurePoints[currentPoint].measured.translation.x += tempPose[i].translation.x;
          measurePoints[currentPoint].measured.translation.y += tempPose[i].translation.y;
          measurePoints[currentPoint].measured.rotation += tempPose[i].rotation;
          dieNackteWahrheit++;
        }

      }
      if(dieNackteWahrheit > 0)
      {
        // hier setze ich für die rotationsmessung mal die translation auf 0
        measurePoints[currentPoint].measured.translation.x /= dieNackteWahrheit;
        measurePoints[currentPoint].measured.translation.y /= dieNackteWahrheit;
        measurePoints[currentPoint].measured.rotation /= dieNackteWahrheit;
      }
      else
      {
        OUTPUT(idText, text, "Illegal value. Repeat measure.");
        result = resultReposition;
        state = stateMeasureStart;
        break;
      }
      //      OUTPUT(idText, text, "indizes: " << tempDeleteMin[0] << "  " << tempDeleteMin[1] << "  " << tempDeleteMax[0] << "  " << tempDeleteMax[1]);
      OUTPUT(idText, text, "count: " << dieNackteWahrheit);

      OUTPUT(idText, text, "Controlled: " << measurePoints[currentPoint].controlled.translation.x << ", " << measurePoints[currentPoint].controlled.translation.y << ", " << measurePoints[currentPoint].controlled.rotation);
      OUTPUT(idText, text, "Measured: " << measurePoints[currentPoint].measured.translation.x << ", " << measurePoints[currentPoint].measured.translation.y << ", " << measurePoints[currentPoint].measured.rotation);
      if(qCount > 0)
        OUTPUT(idText, text, "Last rms error: " << sqrt(qSum/(double)qCount));
      OUTPUT(idText, text, "");

      //      if(fabs(normalize(rotationStartAngle - rotationStopAngle) / 3) < 0.25)//0.125)
      currentPoint++;
      measureCount++;
      //      else
      //        OUTPUT(idText, text, "Repeat Measure, because Robot rotated too much");


      if(currentPoint < measureSize)
      {
        // write so far measured points to file
        OutBinaryFile file("_Measure.bin");
        for(int i = 0; i < currentPoint; i++) //currentPoint is already incremented
          file << measurePoints[i].controlled << measurePoints[i].measured;

        OUTPUT(idText, text, "GT2005 Walk-Calibration done measure #" << currentPoint - 1);

        state = stateMeasureStart;
        result = resultReposition;
        startPoseBadCount = 0;
      }
      else
      {
        OutBinaryFile deleteFile("_Measure.bin"); // shrink file to zer0 size
        OutBinaryFile file(filename);
        for(int i = 0; i < measureSize; i++)
          file << measurePoints[i].controlled << measurePoints[i].measured;

        OUTPUT(idText, text, "GT2005 Walk-Calibration finished");
        state = stateCalibrationFinished;
        result = resultUnready;
      }
      break;
    }
   case stateCalibrationFinished:
    OUTPUT(idText, text, "calibration finished.");
    result = resultUnready;
    state = stateCalibrationFinished;
    return;

  case stateNotInitialized:
    OUTPUT(idText, text, "not yet initialized.");
    result = resultUnready;
    state = stateNotInitialized;
    return;

  default:
    OUTPUT(idText, text, "unknown state " << state);
    result = resultUnready;
    return;
  }
}


double GT2005WalkCalibrationMainBehavior::minDistanceToBorder(Vector2<double>& t)
{
  double dist;

  if(t.x < 0.0 || t.x > 2400.0 || fabs(t.y) > 1900.0)
    return 0.0;

  dist = fabs(2400.0 - t.x);
  dist = min(dist, t.x);
  dist = min(dist, 1900.0 - fabs(t.y));

  return dist;
}


Pose2D GT2005WalkCalibrationMainBehavior::calculateStartPose(const Pose2D& base)
{
  const int count = 9; //zeitwert
  double dist1, dist2, dist3;
  double maxDist1 = 0, minDist2 = 10000;

  Pose2D bestPose;
  Pose2D pose[count];

  for(double r = -pi; r <= pi; r += pi / 40.0)
  {
    for(int x = 100; x <= 2300; x += 200)
    {
      for(int y = -1800; y <= 1800; y += 200)
      {
        pose[0].rotation = r;
        pose[0].translation.x = x;
        pose[0].translation.y = y;
        dist1 = 10000.0;
        dist2 = 0.0;

        for(int i = 1; i < count; i++)
          pose[i] = pose[i - 1] + base;

        //abstand zum border
        for(int i = 0; i < count; i++)
          dist1 = min(dist1, minDistanceToBorder(pose[i].translation));

        //stdabweichung abstand zur kamera, erste beiden punkte ignorieren dabei
        if(count > 2)
        {
          for(int i = 2; i < count; i++)
          {
            double dist = (pose[i].translation - center).abs();
            dist2 += sqrt(dist*dist);
          }
          dist2 /= (double)(count - 2);
        }

        //abstand zum startpunkt
        dist3 = (pose[0].translation - camRobotPose.translation).abs();
        dist2 += dist3 * 0.5; //dist3 halb gewichten

        if((dist1 > maxDist1)||
          (dist1 == maxDist1 && dist2 < minDist2))
        {
          maxDist1 = dist1;
          minDist2 = dist2;
          bestPose = pose[0];
        }
      }
    }
  }

  pose[0] = bestPose;
  for(int i = 0; i < count; i++)
  {  
    if(i == 0)
    {
      NCIRCLE("GT2005WalkCalibration:walkpath", pose[i].translation.x, pose[i].translation.y, 15,1,Drawings::ps_solid,Drawings::blue);
    }
    else
    {
      pose[i] = pose[i - 1] + base;
      NCIRCLE("GT2005WalkCalibration:walkpath", pose[i].translation.x, pose[i].translation.y, 15,1,Drawings::ps_solid,Drawings::white);
    }
  }
  NCROSS("GT2005WalkCalibration:walkpath", center.x, center.y, 50, 10, Drawings::ps_solid, Drawings::black);

  return bestPose;
}




void GT2005WalkCalibrationMainBehavior::registerSymbols(Xabsl2Engine& engine)
{
  engine.registerDecimalInputSymbol("walk-calibration.result", &result);
  engine.registerDecimalInputSymbol("walk-calibration.start.x", &startPose.translation.x);
  engine.registerDecimalInputSymbol("walk-calibration.start.y", &startPose.translation.y);
  engine.registerDecimalInputSymbol("walk-calibration.start.angle", &startPoseAngle);
}

void GT2005WalkCalibrationBasicBehaviors::registerBasicBehaviors(Xabsl2Engine& engine)
{
  engine.registerBasicBehavior(gt2005WalkCalibrationMainBehavior);
  gt2005WalkCalibrationMainBehavior.registerSymbols(engine);
}
//TODO bessere borderbeststimmung bzw abstand(stdabweichung) zum mittelpunkt
//TODO flag setzten ob startPos geschätzt oder gemessen
//TODO abbrechen wenn geschätzt und rms > 1000 zwischendurch

---