Modules/WalkingEngine/GT2005WalkingEngine.cpp

Go to the documentation of this file.

/***************************************************************************
* @file GT2005WalkingEngine.cpp
* class GT2005WalkingEngine
* @author Denis Fisseler
****************************************************************************/

#include "GT2005WalkingEngine.h"
#include "Tools/Player.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "InvKinWalkingParameterSets.h"
#include "Tools/Debugging/Debugging.h"
#include "GT2004ParameterSet.h"
#include "Tools/Actorics/RobotVertices.h"

const int GT2005WalkingEngine::Leg_FR = 0;
const int GT2005WalkingEngine::Leg_FL = 1;
const int GT2005WalkingEngine::Leg_HR = 2;
const int GT2005WalkingEngine::Leg_HL = 3;

GT2005WalkingEngine::GT2005WalkingEngine(const WalkingEngineInterfaces& interfaces):
WalkingEngine(interfaces), currentRequest(0,0,0), currentStepPercentage(0)
{
  OUTPUT(idText, text, "");
  OUTPUT(idText, text, "GT2005WalkingEngine: loading...");

  loadingFailed = false;
  activePoints = 4;

  neckHeight = 180;
  bodyTilt = 0;
  currentStepPercentage = 0;
  currentStep = 0;
  currentParameters = paramSet.mergedParameters; // only needed for head/tail movement
  standing = false;

  debugging = false;
  debugRequest = false;
  debugLoop = true;

  resampleRequest = false;
  resampling = false;

  polygonsRecieved = 0;

  currentFIPSParameters = new GT2005Parameters();

  if (currentFIPSParameters->loadingFailed)
  {
    loadingFailed = true;
    OUTPUT(idText, text, "GT2005WalkingEngine: Loading of parameter set failed!");
  }

  // nur ne initialisierung, damit er sich beim ersten delete nicht aufhängt
  currentPoly[Leg_FR] = new GT2005Polygon(1);
  currentPoly[Leg_FL] = new GT2005Polygon(1);
  currentPoly[Leg_HR] = new GT2005Polygon(1);
  currentPoly[Leg_HL] = new GT2005Polygon(1);

  gt2005DebugData.gt2005Status = 0;

  readOdometryCalibrationData();

  if (loadingFailed)
  {
    OUTPUT(idText, text, "GT2005WalkingEngine: Not successfuly loaded!");
  }
  else
  {
    OUTPUT(idText, text, "GT2005WalkingEngine: Successfuly loaded! Have a nice walk!");
  }
  OUTPUT(idText, text, "");
}

GT2005WalkingEngine::~GT2005WalkingEngine()
{
  for (int i = 0; i < 4; i++)
  {
    delete currentPoly[i];
  }
  delete currentFIPSParameters;
}

void GT2005WalkingEngine::loadOdometryPlane(int planeNum, char* filename, int alternateDir)
{
  int i, j;

  char o_filename[30];
  sprintf(o_filename, "mshwep/%02u/%s", alternateDir, filename);

  // load special odometry if available
  InBinaryFile ofile(o_filename);
  if (ofile.exists() && (alternateDir > 0))
  {
    for(i = 0; i < 15; i++)
    {
      for(j = 0; j < 13; j++)
      {
        ofile >> odometryCalibrationData[i][j][planeNum].controlled >> odometryCalibrationData[i][j][planeNum].measured;
        // reset for standing
        if (odometryCalibrationData[i][j][planeNum].controlled == Pose2D(0.0, 0.0, 0.0))
        {
          odometryCalibrationData[i][j][planeNum].measured = Pose2D(0.0, 0.0, 0.0);
        }
      }
    }
    OUTPUT(idText, text, "GT2005WalkingEngine: Alternative odometry parameters loaded. (" << o_filename << ")");
  }
  else // load default odometry
  {
    char o_filename_default[30];
    sprintf(o_filename_default, "mshwep/%s", filename);

    InBinaryFile ofile_default(o_filename_default);
    if (ofile_default.exists())
    {
      for(i = 0; i < 15; i++)
      {
        for(j = 0; j < 13; j++)
        {
          ofile_default >> odometryCalibrationData[i][j][planeNum].controlled >> odometryCalibrationData[i][j][planeNum].measured;
          // reset for standing
          if (odometryCalibrationData[i][j][planeNum].controlled == Pose2D(0.0, 0.0, 0.0))
          {
            odometryCalibrationData[i][j][planeNum].measured = Pose2D(0.0, 0.0, 0.0);
          }
        }
      }
    }
    else
    {
      loadingFailed = false;
      OUTPUT(idText, text, "GT2005WalkingEngine: Loading of odometry parameters failed! (" << o_filename_default << ")");
    }
  }
}

void GT2005WalkingEngine::loadOdometryRotation(int rotNum, char* filename, int alternateDir)
{
  int i;

  char o_filename[30];
  sprintf(o_filename, "mshwep/%02u/%s", alternateDir, filename);

  // load special odometry if available
  InBinaryFile ofile(o_filename);
  if (ofile.exists() && (alternateDir > 0))
  {
    for(i = 0; i < 21; i++)
    {
      ofile >> odometryCalibrationData2[rotNum][i].controlled >> odometryCalibrationData2[rotNum][i].measured;
      if (odometryCalibrationData2[rotNum][i].controlled.rotation == 0.0)
      {
        odometryCalibrationData2[rotNum][i].controlled = odometryCalibrationData2[rotNum][i].measured = Pose2D(0.0, 0.0, 0.0);
      }
    }
    OUTPUT(idText, text, "GT2005WalkingEngine: Alternative odometry parameters loaded. (" << o_filename << ")");
  }
  else // load default odometry
  {
    char o_filename_default[30];
    sprintf(o_filename_default, "mshwep/%s", filename);

    InBinaryFile ofile_default(o_filename_default);
    if (ofile_default.exists())
    {
      for(i = 0; i < 21; i++)
      {
        ofile_default >> odometryCalibrationData2[rotNum][i].controlled >> odometryCalibrationData2[rotNum][i].measured;
        if (odometryCalibrationData2[rotNum][i].controlled.rotation == 0.0)
        {
          odometryCalibrationData2[rotNum][i].controlled = odometryCalibrationData2[rotNum][i].measured = Pose2D(0.0, 0.0, 0.0);
        }
      }
    }
    else
    {
      loadingFailed = false;
      OUTPUT(idText, text, "GT2005WalkingEngine: Loading of odometry parameters failed! (" << o_filename_default << ")");
    }
  }
}

void GT2005WalkingEngine::readOdometryCalibrationData()
{
  // load odometry mac dependent
  int d = 0;
  unsigned char address[6];
  SystemCall::getMacAddress(address);
  char macAddress[13];
  sprintf(macAddress, "%02X%02X%02X%02X%02X%02X", int(address[0]), int(address[1]), int(address[2]), int(address[3]), int(address[4]), int(address[5]));
  InConfigFile macCFG("mshwep/mac.dat", macAddress);
  if(macCFG.exists() && !macCFG.eof())
  {
    char ignore[50];
    macCFG >> ignore >> d;
  }
  OUTPUT(idText, text, "GT2005WalkingEngine: Robot recognised: " << macAddress);

  loadOdometryPlane(0, "od_200m.dat", d);
  loadOdometryPlane(1, "od_100m.dat", d);
  loadOdometryPlane(2, "od_50m.dat", d);
  loadOdometryPlane(3, "od_0.dat", d);
  loadOdometryPlane(4, "od_50.dat", d);
  loadOdometryPlane(5, "od_100.dat", d);
  loadOdometryPlane(6, "od_200.dat", d);
  loadOdometryRotation(0, "od_r.dat", d);
  loadOdometryRotation(1, "od_rg.dat", d);
}

Pose2D GT2005WalkingEngine::calculateOdometryCalibrationData(Pose2D request, WalkRequest::WalkType walkType)
{
  Pose2D temp;
  int i = 0;
  int j = 0;
  int k = 0;
  bool oSet = false;

  if ((fabs(request.rotation) > 3.4906) || ((request.translation.x == 0.0) && (request.translation.y == 0.0))) // old rotation calibration
  {
    // rotation stuff
    if (walkType == WalkRequest::walkWithBall)
    {
      for (i = 1; i < 21; i++)
      {
        if (request.rotation <= odometryCalibrationData2[1][i].controlled.rotation)
        {
          double dr = odometryCalibrationData2[1][i].controlled.rotation - odometryCalibrationData2[1][i-1].controlled.rotation;
          double pr = request.rotation - odometryCalibrationData2[1][i-1].controlled.rotation;

          //temp.translation.x = (1 - (pr / dr)) * odometryCalibrationData2[1][i-1].measured.translation.x + (pr / dr) * odometryCalibrationData2[1][i].measured.translation.x;
          //temp.translation.y = (1 - (pr / dr)) * odometryCalibrationData2[1][i-1].measured.translation.y + (pr / dr) * odometryCalibrationData2[1][i].measured.translation.y;
          temp.rotation = (1 - (pr / dr)) * odometryCalibrationData2[1][i-1].measured.rotation + (pr / dr) * odometryCalibrationData2[1][i].measured.rotation;
          oSet = true;
          break;
        }
      }
      if (oSet == false)
      {
        //temp.translation.x = odometryCalibrationData2[1][20].measured.translation.x;
        //temp.translation.y = odometryCalibrationData2[1][20].measured.translation.y;
        temp.rotation = odometryCalibrationData2[1][20].measured.rotation;
      }
      // blöde Berechnung, um ne reine Rotationsbewegung zu korrigieren
      temp.rotation *= motionCycleTime;
      temp.translation.y += sin(temp.rotation) * 80;
      temp.rotation /= motionCycleTime;
      temp.translation /= motionCycleTime;
    }
    else
    {
      for (i = 1; i < 21; i++)
      {
        if (request.rotation <= odometryCalibrationData2[0][i].controlled.rotation)
        {
          double dr = odometryCalibrationData2[0][i].controlled.rotation - odometryCalibrationData2[0][i-1].controlled.rotation;
          double pr = request.rotation - odometryCalibrationData2[0][i-1].controlled.rotation;

          //temp.translation.x = (1 - (pr / dr)) * odometryCalibrationData2[0][i-1].measured.translation.x + (pr / dr) * odometryCalibrationData2[0][i].measured.translation.x;
          //temp.translation.y = (1 - (pr / dr)) * odometryCalibrationData2[0][i-1].measured.translation.y + (pr / dr) * odometryCalibrationData2[0][i].measured.translation.y;
          temp.rotation = (1 - (pr / dr)) * odometryCalibrationData2[0][i-1].measured.rotation + (pr / dr) * odometryCalibrationData2[0][i].measured.rotation;
          oSet = true;
          break;
        }
      }
      if (oSet == false)
      {
        //temp.translation.x = odometryCalibrationData2[0][20].measured.translation.x;
        //temp.translation.y = odometryCalibrationData2[0][20].measured.translation.y;
        temp.rotation = odometryCalibrationData2[0][20].measured.rotation;
      }
      // blöde Berechnung, um ne reine Rotationsbewegung zu korrigieren
      temp.rotation *= motionCycleTime;
      temp.translation.y += sin(temp.rotation) * 80;
      temp.rotation /= motionCycleTime;
      temp.translation /= motionCycleTime;
    }
    return temp;
  }
  else
  {
    oSet = false;
    // translation stuff
    for (i = 1; i < 15; i++)
    {
      if (request.translation.x <= odometryCalibrationData[i][0][0].controlled.translation.x)
      {
        double dx = odometryCalibrationData[i][0][0].controlled.translation.x - odometryCalibrationData[i-1][0][0].controlled.translation.x;
        double px = request.translation.x - odometryCalibrationData[i-1][0][0].controlled.translation.x;
        for (j = 1; j < 13; j++)
        {
          if (request.translation.y <= odometryCalibrationData[i][j][0].controlled.translation.y)
          {
            // lengths of current segments
            double dy = odometryCalibrationData[i][j][0].controlled.translation.y - odometryCalibrationData[i][j-1][0].controlled.translation.y;
            // positions in current segments
            double py = request.translation.y - odometryCalibrationData[i][j-1][0].controlled.translation.y;

            for (k = 1; k < 7; k++)
            {
              // lengths of current segments
              double dr = odometryCalibrationData[i][j][k].controlled.rotation - odometryCalibrationData[i][j][k-1].controlled.rotation;
              // positions in current segments
              double pr = request.rotation - odometryCalibrationData[i][j][k-1].controlled.rotation;

              if (request.rotation <= odometryCalibrationData[i][j][k].controlled.rotation)
              {
                double wx = px / dx;
                double wy = py / dy;
                double wr = pr / dr;

                // linear interpolation
                temp.translation.x = (1 - wr) * ((1 - wy) * ((1 - wx) * odometryCalibrationData[i-1][j-1][k-1].measured.translation.x + wx * odometryCalibrationData[i][j-1][k-1].measured.translation.x)
                  + wy * ((1 - wx) * odometryCalibrationData[i-1][j][k-1].measured.translation.x + wx * odometryCalibrationData[i][j][k-1].measured.translation.x))
                  + wr * ((1 - wy) * ((1 - wx) * odometryCalibrationData[i-1][j-1][k].measured.translation.x + wx * odometryCalibrationData[i][j-1][k].measured.translation.x)
                  + wy * ((1 - wx) * odometryCalibrationData[i-1][j][k].measured.translation.x + wx * odometryCalibrationData[i][j][k].measured.translation.x));

                temp.translation.y = (1 - wr) * ((1 - wx) * ((1 - wy) * odometryCalibrationData[i-1][j-1][k-1].measured.translation.y + wy * odometryCalibrationData[i-1][j][k-1].measured.translation.y)
                  + wx * ((1 - wy) * odometryCalibrationData[i][j-1][k-1].measured.translation.y + wy * odometryCalibrationData[i][j][k-1].measured.translation.y))
                  + wr * ((1 - wx) * ((1 - wy) * odometryCalibrationData[i-1][j-1][k].measured.translation.y + wy * odometryCalibrationData[i-1][j][k].measured.translation.y)
                  + wx * ((1 - wy) * odometryCalibrationData[i][j-1][k].measured.translation.y + wy * odometryCalibrationData[i][j][k].measured.translation.y));

                temp.rotation = (1 - wr) * ((1 - wy) * ((1 - wx) * odometryCalibrationData[i-1][j-1][k-1].measured.rotation + wx * odometryCalibrationData[i][j-1][k-1].measured.rotation)
                  + wy * ((1 - wx) * odometryCalibrationData[i-1][j][k-1].measured.rotation + wx * odometryCalibrationData[i][j][k-1].measured.rotation))
                  + wr * ((1 - wy) * ((1 - wx) * odometryCalibrationData[i-1][j-1][k].measured.rotation + wx * odometryCalibrationData[i][j-1][k].measured.rotation)
                  + wy * ((1 - wx) * odometryCalibrationData[i-1][j][k].measured.rotation + wx * odometryCalibrationData[i][j][k].measured.rotation));
                oSet = true;
                return temp;
              }
            }
          }
        }
      }
    }
    if (oSet == false)
    {
      temp.translation.x = odometryCalibrationData[14][12][6].measured.translation.x;
      temp.translation.y = odometryCalibrationData[14][12][6].measured.translation.y;
      temp.rotation = odometryCalibrationData[14][12][6].measured.rotation;
    }
  }

  return Pose2D(0.0, 0.0, 0.0);
}

bool GT2005WalkingEngine::executeParameterized(JointData& jointData, const WalkRequest& walkRequest, double positionInWalkingCycle)
{
  // set PID values
  pidData.setLegFJ1Values(38, 8, 1);
  pidData.setLegHJ1Values(38, 8, 1);
  pidData.setLegFJ2Values(30, 4, 1);
  pidData.setLegHJ2Values(30, 4, 1);
  pidData.setLegFJ3Values(40, 8, 1);
  pidData.setLegHJ3Values(40, 8, 1);

  // evolution message handling
  switch (gt2005Parameters.bMessage)
  {
  case 0: // reset special mode for rotation
    {
      currentFIPSParameters->specialMode = false;
      gt2005DebugData.gt2005Status = 0;
      polygonsRecieved = 0;
      break;
    }
  case 1: // integrate polygons send from evolution
    {
      if (polygonsRecieved == 0)
      {
        delete currentFIPSParameters->evoPoly[Leg_FR];
        delete currentFIPSParameters->evoPoly[Leg_FL];
        delete currentFIPSParameters->evoPoly[Leg_HR];
        delete currentFIPSParameters->evoPoly[Leg_HL];

        currentFIPSParameters->evoPoly[Leg_FR] = gt2005Parameters.walkingParameters.evoPoly[Leg_FR]->copy();
        currentFIPSParameters->evoPoly[Leg_FL] = gt2005Parameters.walkingParameters.evoPoly[Leg_FL]->copy();
        currentFIPSParameters->evoPoly[Leg_HR] = gt2005Parameters.walkingParameters.evoPoly[Leg_HR]->copy();
        currentFIPSParameters->evoPoly[Leg_HL] = gt2005Parameters.walkingParameters.evoPoly[Leg_HL]->copy();
        polygonsRecieved = 1;
        gt2005DebugData.gt2005Status = 1; // signalize "polygons recieved" to behaviour
        //OUTPUT(idText, text, "GT2005WalkingEngine: Polygons recieved.");
      }

      break;
    }
  case 2: // switch to special mode for rotation
    {
      currentFIPSParameters->specialMode = true;
      break;
    }
  default:
    {
      gt2005DebugData.gt2005Status = 0;
      break;
    }
  }

  // actualize leg phases
  footPhase[Leg_FR] = currentFIPSParameters->footPhase[Leg_FR];
  footPhase[Leg_FL] = currentFIPSParameters->footPhase[Leg_FL];
  footPhase[Leg_HR] = currentFIPSParameters->footPhase[Leg_HR];
  footPhase[Leg_HL] = currentFIPSParameters->footPhase[Leg_HL];

  // adjust controled speed to fit real speed
  Pose2D request = walkRequest.walkParams;
  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
  //currentRequest = request;
  smoothMotionRequest(request, currentRequest);

  // odometry correction
  //currentRequest = MSH2005WalkCalibration::calibrateRequest(currentRequest);

  // clip values
  if (currentRequest.translation.x > 300.0) currentRequest.translation.x = 300.0;
  else if (currentRequest.translation.x < -300.0) currentRequest.translation.x = -300.0;
  if (currentRequest.translation.y > 300.0) currentRequest.translation.y = 300.0;
  else if (currentRequest.translation.y < -300.0) currentRequest.translation.y = -300.0;
  if (currentRequest.rotation > 5.236) currentRequest.rotation = 5.236;
  else if (currentRequest.rotation < -5.236) currentRequest.rotation = -5.236;

  standing = (currentRequest.translation.abs() < 10 && fabs(currentRequest.rotation) < 0.05);

  // calculate current polygons and set current stepLength
  delete currentPoly[Leg_FR]; delete currentPoly[Leg_FL]; delete currentPoly[Leg_HR]; delete currentPoly[Leg_HL];
  currentPoly[Leg_FR] = currentFIPSParameters->calculateCurrentPoly(Leg_FR, currentRequest, walkRequest.walkType);
  currentPoly[Leg_FL] = currentFIPSParameters->calculateCurrentPoly(Leg_FL, currentRequest, walkRequest.walkType);
  currentPoly[Leg_HR] = currentFIPSParameters->calculateCurrentPoly(Leg_HR, currentRequest, walkRequest.walkType);
  currentPoly[Leg_HL] = currentFIPSParameters->calculateCurrentPoly(Leg_HL, currentRequest, walkRequest.walkType);
  currentFIPSParameters->stepLength = currentPoly[Leg_FR]->stepLength;

  // calculate current position in walk cycle if walking
  if (standing)
  {
    footPhase[Leg_FR] = footPhase[Leg_FL] = footPhase[Leg_HR] = footPhase[Leg_HL] = 0;

    // end current walk cycle
    currentStepPercentage = positionInWalkingCycle;
    if (currentStepPercentage != 0.0)
    {
      currentStepPercentage += 1.0 / (double)currentFIPSParameters->stepLength;
      currentStep++;
      if (currentStepPercentage >= 1.0)
      {
        currentStepPercentage -= 1.0;
        currentStep = 0;
      }
    }
  }
  else
  {
    currentStepPercentage = positionInWalkingCycle;
    currentStepPercentage += 1.0 / (double)currentFIPSParameters->stepLength;
    currentStep++;
    if (currentStepPercentage >= 1.0)
    {
      currentStepPercentage -= 1.0;
      currentStep = 0;

      // switch debugging mode
      if (debugRequest && (debugging == false))
      {
        debugging = true;
        OUTPUT(idText, text, "GT2005WalkingEngine: Debugging started.");
      }
      else if ((debugRequest == false) && debugging)
      {
        debugging = false;
        streamDebugData();
        OUTPUT(idText, text, "GT2005WalkingEngine: Debugging stopped.");
      }

      if (resampleRequest && (resampling == false))
      {
        resampling = true;
        OUTPUT(idText, text, "GT2005WalkingEngine: Resampling started.");
      }
      else if ((resampleRequest == false) && resampling)
      {
        resampling = false;
        streamDebugData();
        OUTPUT(idText, text, "GT2005WalkingEngine: Resampling stopped.");
      }
    }
  }

  // fake odometry ;)
  odometry = calculateOdometryCalibrationData(currentRequest, walkRequest.walkType);
  //OUTPUT(idText, text, "tr: " << odometry.translation.y << "  rot: " << odometry.rotation);
  //odometry = request; // original request

  //convert odometry from mm/s to mm/frame
  odometry.translation *= motionCycleTime;
  //convert odometry from rad/s to rad/frame
  odometry.rotation *= motionCycleTime;
  //correction for rotation center = foot middle instead of neck:
  //odometry.translation.y += sin(odometry.rotation) * 75;
  odometryData.conc(odometry);

  // calculate body tilt (funzt nur wenn die footZeroPositions für links und rechts gleich sind)
  bodyTilt = asin((fabs(currentFIPSParameters->footZeroPosition[Leg_HR].z) - fabs(currentFIPSParameters->footZeroPosition[Leg_FR].z)) / lengthBetweenLegs);

  // calculate foot positions
  calculateData(jointData);

  // set motion info stuff
  motionInfo.neckHeight = neckHeight;
  motionInfo.motionIsStable = true;
  motionInfo.bodyTilt = bodyTilt + getRobotConfiguration().getRobotCalibration().bodyTiltOffset;
  motionInfo.executedMotionRequest.motionType = MotionRequest::walk;
  motionInfo.executedMotionRequest.walkRequest.walkType = walkRequest.walkType;
  motionInfo.executedMotionRequest.walkRequest.walkParams = currentRequest;
  motionInfo.positionInWalkCycle = currentStepPercentage;

  /*/ override head control mode
  if (currentFIPSParameters->specialMode)
  {
  //pidData.setValues(JointData::neckTilt, 0x0A, 0x04, 0x02);
  jointData.data[JointData::headTilt] = toMicroRad(fromDegrees(0.0));
  jointData.data[JointData::headPan] = toMicroRad(fromDegrees(0.0));
  jointData.data[JointData::neckTilt] = toMicroRad(fromDegrees(0.0));
  jointData.data[JointData::mouth] = toMicroRad(fromDegrees(0.0));
  }
  else
  {
  //pidData.setValues(JointData::neckTilt, 0x00, 0x00, 0x00);
  jointData.data[JointData::headTilt] = toMicroRad(fromDegrees(50.0));
  jointData.data[JointData::headPan] = toMicroRad(fromDegrees(0.0));
  jointData.data[JointData::neckTilt] = toMicroRad(fromDegrees(-60.0));
  jointData.data[JointData::mouth] = toMicroRad(fromDegrees(-55.0));
  }//*/

  return false;
}

void GT2005WalkingEngine::calculateData(JointData &j) 
{
  double j1,j2,j3;
  double shapeOffset[4];
  double a[4];
  double vLen[4];
  double speedScaleFactor[4];

  //head, mouth
  j.data[JointData::neckTilt] = currentParameters->headTilt;
  j.data[JointData::headPan] = currentParameters->headPan;
  j.data[JointData::headTilt] = currentParameters->headRoll;
  j.data[JointData::mouth] = currentParameters->mouth;

  // ears, tail
  j.data[JointData::earL] =j.data[JointData::earR] = j.data[JointData::tailTilt] = j.data[JointData::tailPan] = jointDataInvalidValue;

  // get foot positions
  Vector3<double> footPos[4];
  footPos[Leg_FR] = currentFIPSParameters->footZeroPosition[Leg_FR];
  footPos[Leg_FL] = currentFIPSParameters->footZeroPosition[Leg_FL];
  footPos[Leg_HR] = currentFIPSParameters->footZeroPosition[Leg_HR];
  footPos[Leg_HL] = currentFIPSParameters->footZeroPosition[Leg_HL];

  if (standing == false) // if moving-
  {
    // calculate shapeOffsets
    shapeOffset[Leg_FR] = currentStepPercentage + footPhase[Leg_FR];
    shapeOffset[Leg_FL] = currentStepPercentage + footPhase[Leg_FL];
    shapeOffset[Leg_HR] = currentStepPercentage + footPhase[Leg_HR];
    shapeOffset[Leg_HL] = currentStepPercentage + footPhase[Leg_HL];
    for (int i = 0; i < 4; i++)
    {
      if (shapeOffset[i] > 1)
      {
        shapeOffset[i] -= 1;
      }
    }

    // define rotation vectors
    double rotLen = currentPoly[Leg_FR]->polyLengthX;
    Vector3<double> footRotation[4];
    if (currentRequest.rotation < 0) // rotate counterclockwise
    {
      footRotation[Leg_FR].x = -rotLen; footRotation[Leg_FR].y = -rotLen; footRotation[Leg_FR].z = 0;
      footRotation[Leg_FL].x = rotLen;  footRotation[Leg_FL].y = -rotLen; footRotation[Leg_FL].z = 0;
      footRotation[Leg_HR].x = -rotLen; footRotation[Leg_HR].y = rotLen;  footRotation[Leg_HR].z = 0;
      footRotation[Leg_HL].x = rotLen;  footRotation[Leg_HL].y = rotLen;  footRotation[Leg_HL].z = 0;
    }
    else // rotate clockwise
    {
      footRotation[Leg_FR].x = rotLen;  footRotation[Leg_FR].y = rotLen;  footRotation[Leg_FR].z = 0;
      footRotation[Leg_FL].x = -rotLen; footRotation[Leg_FL].y = rotLen;  footRotation[Leg_FL].z = 0;
      footRotation[Leg_HR].x = rotLen;  footRotation[Leg_HR].y = -rotLen; footRotation[Leg_HR].z = 0;
      footRotation[Leg_HL].x = -rotLen; footRotation[Leg_HL].y = -rotLen; footRotation[Leg_HL].z = 0;
    }

    // define translation vector
    Vector3<double> v;
    v.x = currentRequest.translation.x;
    v.y = currentRequest.translation.y;
    v.z = 0;

    // calculate robot rotation scale factor
    double maxRot = 1.0; // maximum rotation (sorry geschätzt!!!)
    double rsf = currentRequest.rotation / maxRot;
    if (rsf < 0)
    {
      rsf *= -1;
    }

    // scale rotation vectors
    footRotation[Leg_FR] *= rsf;
    footRotation[Leg_FL] *= rsf;
    footRotation[Leg_HR] *= rsf;
    footRotation[Leg_HL] *= rsf;

    // put translation vectors on rotation vectors
    footRotation[Leg_FR] += v;
    footRotation[Leg_FL] += v;
    footRotation[Leg_HR] += v;
    footRotation[Leg_HL] += v;

    // calculate directions of the combined translation+rotation vectors
    a[Leg_FR] = atan2(footRotation[Leg_FR].y, footRotation[Leg_FR].x);
    a[Leg_FL] = atan2(footRotation[Leg_FL].y, footRotation[Leg_FL].x);
    a[Leg_HR] = atan2(footRotation[Leg_HR].y, footRotation[Leg_HR].x);
    a[Leg_HL] = atan2(footRotation[Leg_HL].y, footRotation[Leg_HL].x);

    // calculeate length of the combined translation+rotation vectors
    vLen[Leg_FR] = sqrt(footRotation[Leg_FR].x * footRotation[Leg_FR].x + footRotation[Leg_FR].y * footRotation[Leg_FR].y);
    vLen[Leg_FL] = sqrt(footRotation[Leg_FL].x * footRotation[Leg_FL].x + footRotation[Leg_FL].y * footRotation[Leg_FL].y);
    vLen[Leg_HR] = sqrt(footRotation[Leg_HR].x * footRotation[Leg_HR].x + footRotation[Leg_HR].y * footRotation[Leg_HR].y);
    vLen[Leg_HL] = sqrt(footRotation[Leg_HL].x * footRotation[Leg_HL].x + footRotation[Leg_HL].y * footRotation[Leg_HL].y);

    // calculate resulting speed scale factor
    speedScaleFactor[0] = speedScaleFactor[1] = speedScaleFactor[2] = speedScaleFactor[3] = 1;
    speedScaleFactor[Leg_FR] = vLen[Leg_FR] / (currentPoly[Leg_FR]->polyLengthX / (currentFIPSParameters->stepLength * motionCycleTime * currentPoly[Leg_FR]->groundTime));
    speedScaleFactor[Leg_FL] = vLen[Leg_FL] / (currentPoly[Leg_FL]->polyLengthX / (currentFIPSParameters->stepLength * motionCycleTime * currentPoly[Leg_FL]->groundTime));
    speedScaleFactor[Leg_HR] = vLen[Leg_HR] / (currentPoly[Leg_HR]->polyLengthX / (currentFIPSParameters->stepLength * motionCycleTime * currentPoly[Leg_HR]->groundTime));
    speedScaleFactor[Leg_HL] = vLen[Leg_HL] / (currentPoly[Leg_HL]->polyLengthX / (currentFIPSParameters->stepLength * motionCycleTime * currentPoly[Leg_HL]->groundTime));

    // calculate foot positions on polygon (use zero positions for standing)
    footPos[Leg_FR] += currentPoly[Leg_FR]->scaleX(speedScaleFactor[Leg_FR]).rotateZC(a[Leg_FR]).getPositionOnShape(shapeOffset[Leg_FR]);
    footPos[Leg_FL] += currentPoly[Leg_FL]->scaleX(speedScaleFactor[Leg_FL]).rotateZC(a[Leg_FL]).getPositionOnShape(shapeOffset[Leg_FL]);
    footPos[Leg_HR] += currentPoly[Leg_HR]->scaleX(speedScaleFactor[Leg_HR]).rotateZC(a[Leg_HR]).getPositionOnShape(shapeOffset[Leg_HR]);
    footPos[Leg_HL] += currentPoly[Leg_HL]->scaleX(speedScaleFactor[Leg_HL]).rotateZC(a[Leg_HL]).getPositionOnShape(shapeOffset[Leg_HL]);
  }

  // calculate leg angles from foot positions with inverse kinematics and set joint data
  for (int leg = 0; leg < 4; leg++)
  {
    if (Kinematics::jointsFromLegPosition((Kinematics::LegIndex)leg, footPos[leg].x, footPos[leg].y, footPos[leg].z, j1, j2, j3, bodyTilt))
    {
      j.data[JointData::legFR1+leg*3] = toMicroRad(j1);
      j.data[JointData::legFR2+leg*3] = toMicroRad(j2);
      j.data[JointData::legFR3+leg*3] = toMicroRad(j3);
    }
    else
    {
      j.data[JointData::tailTilt] = -300000;
    }
  }

  if (debugging)
  {
    readDebugData(speedScaleFactor, a, shapeOffset, j);
  }
  if (resampling)
  {
    readResampleData(speedScaleFactor, a, shapeOffset, j, footPos);
  }
}

bool GT2005WalkingEngine::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
  case idGT2005Request: 
    {
      int actionID;
      message.bin >> actionID;

      switch(actionID)
      {
      case 0:
        {
          // request debug data
          debugRequest = true;
          break;
        }
      case 1:
        {
          // request resample data
          resampleRequest = true;
          break;
        }
      case 2: // load recieved 4 polygons into a specific polygon set in the walking engine
        {
          int polyID;
          message.bin >> polyID;
          loadPolys(polyID, message);
          OUTPUT(idText, text, "Polygons loaded in polyID: " << polyID);
          break;
        }
      case 3: // send 4 polygons from a specific polygon set in the walking engine
        {
          int polyID;
          message.bin >> polyID;
          sendPolys(polyID);
          OUTPUT(idText, text, "Polygons send from polyID: " << polyID);
          break;
        }
      }
      OUTPUT(idText, text, "Request successful with actionID: " << actionID);
      return true;
    }
  }
  return false;
}

void GT2005WalkingEngine::loadPolys(int polyID, InMessage& message)
{
  switch(polyID)
  {
  case 0: //evoPoly
    {
      message.bin >> *currentFIPSParameters->evoPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->evoPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->evoPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->evoPoly[Leg_HL];
      break;
    }
  case 1: //omniPoly
    {
      message.bin >> *currentFIPSParameters->omniPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->omniPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->omniPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->omniPoly[Leg_HL];
      break;
    }
  case 2: //rotPoly
    {
      message.bin >> *currentFIPSParameters->rotPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->rotPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->rotPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->rotPoly[Leg_HL];
      break;
    }
  case 3: //sforwardPoly
    {
      message.bin >> *currentFIPSParameters->sforwardPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->sforwardPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->sforwardPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->sforwardPoly[Leg_HL];
      break;
    }
  case 4: //forwardPoly
    {
      message.bin >> *currentFIPSParameters->forwardPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->forwardPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->forwardPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->forwardPoly[Leg_HL];
      break;
    }
  case 5: //dforwardPoly
    {
      message.bin >> *currentFIPSParameters->dforwardPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->dforwardPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->dforwardPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->dforwardPoly[Leg_HL];
      break;
    }
  case 6: //sbackwardPoly
    {
      message.bin >> *currentFIPSParameters->sbackwardPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->sbackwardPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->sbackwardPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->sbackwardPoly[Leg_HL];
      break;
    }
  case 7: //backwardPoly
    {
      message.bin >> *currentFIPSParameters->backwardPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->backwardPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->backwardPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->backwardPoly[Leg_HL];
      break;
    }
  case 8: //dbackwardPoly
    {
      message.bin >> *currentFIPSParameters->dbackwardPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->dbackwardPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->dbackwardPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->dbackwardPoly[Leg_HL];
      break;
    }
  case 9: //sidewardsPoly
    {
      message.bin >> *currentFIPSParameters->sidewardsPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->sidewardsPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->sidewardsPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->sidewardsPoly[Leg_HL];
      break;
    }
  case 10: //boostPoly
    {
      message.bin >> *currentFIPSParameters->boostPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->boostPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->boostPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->boostPoly[Leg_HL];
      break;
    }
  case 11: //grabPoly
    {
      message.bin >> *currentFIPSParameters->grabPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->grabPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->grabPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->grabPoly[Leg_HL];
      break;
    }
  case 12: //rgrabPoly
    {
      message.bin >> *currentFIPSParameters->rgrabPoly[Leg_FR];
      message.bin >> *currentFIPSParameters->rgrabPoly[Leg_FL];
      message.bin >> *currentFIPSParameters->rgrabPoly[Leg_HR];
      message.bin >> *currentFIPSParameters->rgrabPoly[Leg_HL];
      break;
    }
  }
}

void GT2005WalkingEngine::sendPolys(int polyID)
{
  // Get debug stream
  Out& out = getDebugOut().bin;

  int dataID = 1;
  // stream data
  out << dataID;

  out << polyID;
  switch(polyID)
  {
  case 0: //evoPoly
    {
      out << *currentFIPSParameters->evoPoly[Leg_FR];
      out << *currentFIPSParameters->evoPoly[Leg_FL];
      out << *currentFIPSParameters->evoPoly[Leg_HR];
      out << *currentFIPSParameters->evoPoly[Leg_HL];
      break;
    }
  case 1: //omniPoly
    {
      out << *currentFIPSParameters->omniPoly[Leg_FR];
      out << *currentFIPSParameters->omniPoly[Leg_FL];
      out << *currentFIPSParameters->omniPoly[Leg_HR];
      out << *currentFIPSParameters->omniPoly[Leg_HL];
      break;
    }
  case 2: //rotPoly
    {
      out << *currentFIPSParameters->rotPoly[Leg_FR];
      out << *currentFIPSParameters->rotPoly[Leg_FL];
      out << *currentFIPSParameters->rotPoly[Leg_HR];
      out << *currentFIPSParameters->rotPoly[Leg_HL];
      break;
    }
  case 3: //sforwardPoly
    {
      out << *currentFIPSParameters->sforwardPoly[Leg_FR];
      out << *currentFIPSParameters->sforwardPoly[Leg_FL];
      out << *currentFIPSParameters->sforwardPoly[Leg_HR];
      out << *currentFIPSParameters->sforwardPoly[Leg_HL];
      break;
    }
  case 4: //forwardPoly
    {
      out << *currentFIPSParameters->forwardPoly[Leg_FR];
      out << *currentFIPSParameters->forwardPoly[Leg_FL];
      out << *currentFIPSParameters->forwardPoly[Leg_HR];
      out << *currentFIPSParameters->forwardPoly[Leg_HL];
      break;
    }
  case 5: //forwardPoly
    {
      out << *currentFIPSParameters->dforwardPoly[Leg_FR];
      out << *currentFIPSParameters->dforwardPoly[Leg_FL];
      out << *currentFIPSParameters->dforwardPoly[Leg_HR];
      out << *currentFIPSParameters->dforwardPoly[Leg_HL];
      break;
    }
  case 6: //sbackwardPoly
    {
      out << *currentFIPSParameters->sbackwardPoly[Leg_FR];
      out << *currentFIPSParameters->sbackwardPoly[Leg_FL];
      out << *currentFIPSParameters->sbackwardPoly[Leg_HR];
      out << *currentFIPSParameters->sbackwardPoly[Leg_HL];
      break;
    }
  case 7: //backwardPoly
    {
      out << *currentFIPSParameters->backwardPoly[Leg_FR];
      out << *currentFIPSParameters->backwardPoly[Leg_FL];
      out << *currentFIPSParameters->backwardPoly[Leg_HR];
      out << *currentFIPSParameters->backwardPoly[Leg_HL];
      break;
    }
  case 8: //backwardPoly
    {
      out << *currentFIPSParameters->dbackwardPoly[Leg_FR];
      out << *currentFIPSParameters->dbackwardPoly[Leg_FL];
      out << *currentFIPSParameters->dbackwardPoly[Leg_HR];
      out << *currentFIPSParameters->dbackwardPoly[Leg_HL];
      break;
    }
  case 9: //sidewardsPoly
    {
      out << *currentFIPSParameters->sidewardsPoly[Leg_FR];
      out << *currentFIPSParameters->sidewardsPoly[Leg_FL];
      out << *currentFIPSParameters->sidewardsPoly[Leg_HR];
      out << *currentFIPSParameters->sidewardsPoly[Leg_HL];
      break;
    }
  case 10: //boostPoly
    {
      out << *currentFIPSParameters->boostPoly[Leg_FR];
      out << *currentFIPSParameters->boostPoly[Leg_FL];
      out << *currentFIPSParameters->boostPoly[Leg_HR];
      out << *currentFIPSParameters->boostPoly[Leg_HL];
      break;
    }
  case 11: //grabPoly
    {
      out << *currentFIPSParameters->grabPoly[Leg_FR];
      out << *currentFIPSParameters->grabPoly[Leg_FL];
      out << *currentFIPSParameters->grabPoly[Leg_HR];
      out << *currentFIPSParameters->grabPoly[Leg_HL];
      break;
    }
  case 12: //rgrabPoly
    {
      out << *currentFIPSParameters->rgrabPoly[Leg_FR];
      out << *currentFIPSParameters->rgrabPoly[Leg_FL];
      out << *currentFIPSParameters->rgrabPoly[Leg_HR];
      out << *currentFIPSParameters->rgrabPoly[Leg_HL];
      break;
    }
  }

  // Finish message
  getDebugOut().finishMessage(idGT2005DebugData);
}

void GT2005WalkingEngine::smoothMotionRequest(const Pose2D& request, Pose2D& currentRequest)
{
  //this is done per frame, so maximum speed change is 3.2mm/s/frame = 400mm/s^2
  //and 0.032rad/s/frame = 4rad/s^2:
  if ((request.translation.x - currentRequest.translation.x) > 25)
  {
    currentRequest.translation.x += 25;
  }
  else if ((request.translation.x - currentRequest.translation.x) < -25)
  {
    currentRequest.translation.x -= 25;
  }
  else
  {
    currentRequest.translation.x = request.translation.x;
  }

  if ((request.translation.y - currentRequest.translation.y) > 25) 
  {
    currentRequest.translation.y += 25;
  }
  else if ((request.translation.y - currentRequest.translation.y) < -25)
  {
    currentRequest.translation.y -= 25;
  }
  else
  {
    currentRequest.translation.y = request.translation.y;
  }

  if ((request.rotation - currentRequest.rotation) > 0.4)
  {
    currentRequest.rotation += 0.4;
  }
  else if ((request.rotation - currentRequest.rotation) < -0.4)
  {
    currentRequest.rotation -= 0.4;
  }
  else
  {
    currentRequest.rotation = request.rotation;
  }
}

void GT2005WalkingEngine::readDebugData(double c_scale[4], double c_angle[4], double c_offset[4], JointData &j)
{
  double x, y, z;
  Vector3<double> v[4];
  Vector3<double> s[4];

  Vector3<double> real[4]; // corrected real vectors from sensor data
  Vector3<double> ideal[4]; // corrected ideal vectors from sensor data

  // define some hardcoded shoulder positions
  Vector3<double> shoulderPosition[4];
  shoulderPosition[Leg_FR].x = 0; shoulderPosition[Leg_FR].y = -67.2; shoulderPosition[Leg_FR].z = -19.5;
  shoulderPosition[Leg_FL].x = 0; shoulderPosition[Leg_FL].y = 67.2;  shoulderPosition[Leg_FL].z = -19.5;
  shoulderPosition[Leg_HR].x = -130;  shoulderPosition[Leg_HR].y = -67.2; shoulderPosition[Leg_HR].z = -19.5;
  shoulderPosition[Leg_HL].x = -130;  shoulderPosition[Leg_HL].y = 67.2;  shoulderPosition[Leg_HL].z = -19.5;

  //sensorDataBuffer.lastFrame().frameNumber

  // read forced angles
  // calculate position in space from joint angles
  Kinematics::legPositionFromJoints(Kinematics::fr,
    fromMicroRad(j.data[JointData::legFR1]),
    fromMicroRad(j.data[JointData::legFR2]),
    fromMicroRad(j.data[JointData::legFR3]),
    x, y, z);
  s[Leg_FR].x = x; s[Leg_FR].y = y - 67.2; s[Leg_FR].z = z;
  // set zero point to footZeroPosition
  s[Leg_FR] -= currentFIPSParameters->footZeroPosition[Leg_FR];
  // undo rotate
  ideal[Leg_FR].x = s[Leg_FR].x * cos(-c_angle[Leg_FR]) - s[Leg_FR].y * sin(-c_angle[Leg_FR]);
  ideal[Leg_FR].y = s[Leg_FR].x * sin(-c_angle[Leg_FR]) + s[Leg_FR].y * cos(-c_angle[Leg_FR]);
  ideal[Leg_FR].z = s[Leg_FR].z;
  // undo x-scale
  ideal[Leg_FR].x = ideal[Leg_FR].x / c_scale[Leg_FR];
  // transform back to foot position
  ideal[Leg_FR] += currentFIPSParameters->footZeroPosition[Leg_FR];
  //ideal[Leg_FR] += shoulderPosition[Leg_FR];

  Kinematics::legPositionFromJoints(Kinematics::fl,
    fromMicroRad(j.data[JointData::legFL1]),
    fromMicroRad(j.data[JointData::legFL2]),
    fromMicroRad(j.data[JointData::legFL3]),
    x, y, z);
  s[Leg_FL].x = x; s[Leg_FL].y = y + 67.2; s[Leg_FL].z = z;
  // set zero point to footZeroPosition
  s[Leg_FL] -= currentFIPSParameters->footZeroPosition[Leg_FL];
  // undo rotate
  ideal[Leg_FL].x = s[Leg_FL].x * cos(-c_angle[Leg_FL]) - s[Leg_FL].y * sin(-c_angle[Leg_FL]);
  ideal[Leg_FL].y = s[Leg_FL].x * sin(-c_angle[Leg_FL]) + s[Leg_FL].y * cos(-c_angle[Leg_FL]);
  ideal[Leg_FL].z = s[Leg_FL].z;
  // undo x-scale
  ideal[Leg_FL].x = ideal[Leg_FL].x / c_scale[Leg_FL];
  // transform back to foot position
  ideal[Leg_FL] += currentFIPSParameters->footZeroPosition[Leg_FL];
  //ideal[Leg_FL] += shoulderPosition[Leg_FL];

  Kinematics::legPositionFromJoints(Kinematics::hr,
    fromMicroRad(j.data[JointData::legHR1]),
    fromMicroRad(j.data[JointData::legHR2]),
    fromMicroRad(j.data[JointData::legHR3]),
    x, y, z);
  s[Leg_HR].x = x; s[Leg_HR].y = y - 67.2; s[Leg_HR].z = z;
  // set zero point to footZeroPosition
  s[Leg_HR] -= currentFIPSParameters->footZeroPosition[Leg_HR];
  // undo rotate
  ideal[Leg_HR].x = s[Leg_HR].x * cos(-c_angle[Leg_HR]) - s[Leg_HR].y * sin(-c_angle[Leg_HR]);
  ideal[Leg_HR].y = s[Leg_HR].x * sin(-c_angle[Leg_HR]) + s[Leg_HR].y * cos(-c_angle[Leg_HR]);
  ideal[Leg_HR].z = s[Leg_HR].z;
  // undo x-scale
  ideal[Leg_HR].x = ideal[Leg_HR].x / c_scale[Leg_HR];
  // transform back to foot position
  ideal[Leg_HR] += currentFIPSParameters->footZeroPosition[Leg_HR];
  //ideal[Leg_HR] += shoulderPosition[Leg_HR];

  Kinematics::legPositionFromJoints(Kinematics::hl,
    fromMicroRad(j.data[JointData::legHL1]),
    fromMicroRad(j.data[JointData::legHL2]),
    fromMicroRad(j.data[JointData::legHL3]),
    x, y, z);
  s[Leg_HL].x = x; s[Leg_HL].y = y + 67.2; s[Leg_HL].z = z;
  // set zero point to footZeroPosition
  s[Leg_HL] -= currentFIPSParameters->footZeroPosition[Leg_HL];
  // undo rotate
  ideal[Leg_HL].x = s[Leg_HL].x * cos(-c_angle[Leg_HL]) - s[Leg_HL].y * sin(-c_angle[Leg_HL]);
  ideal[Leg_HL].y = s[Leg_HL].x * sin(-c_angle[Leg_HL]) + s[Leg_HL].y * cos(-c_angle[Leg_HL]);
  ideal[Leg_HL].z = s[Leg_HL].z;
  // undo x-scale
  ideal[Leg_HL].x = ideal[Leg_HL].x / c_scale[Leg_HL];
  // transform back to foot position
  ideal[Leg_HL] += currentFIPSParameters->footZeroPosition[Leg_HL];
  //ideal[Leg_HL] += shoulderPosition[Leg_HL];

  // read real angles
  int frameNumber = currentStep % 4;

  if (frameNumber == 0)
  {
    debugDataBuffer[0][0] = sensorDataBuffer.frame[0].data[SensorData::legFR1];
    debugDataBuffer[0][1] = sensorDataBuffer.frame[0].data[SensorData::legFR2];
    debugDataBuffer[0][2] = sensorDataBuffer.frame[0].data[SensorData::legFR3];
    debugDataBuffer[0][3] = sensorDataBuffer.frame[1].data[SensorData::legFR1];
    debugDataBuffer[0][4] = sensorDataBuffer.frame[1].data[SensorData::legFR2];
    debugDataBuffer[0][5] = sensorDataBuffer.frame[1].data[SensorData::legFR3];
    debugDataBuffer[0][6] = sensorDataBuffer.frame[2].data[SensorData::legFR1];
    debugDataBuffer[0][7] = sensorDataBuffer.frame[2].data[SensorData::legFR2];
    debugDataBuffer[0][8] = sensorDataBuffer.frame[2].data[SensorData::legFR3];
    debugDataBuffer[0][9] = sensorDataBuffer.frame[3].data[SensorData::legFR1];
    debugDataBuffer[0][10] = sensorDataBuffer.frame[3].data[SensorData::legFR2];
    debugDataBuffer[0][11] = sensorDataBuffer.frame[3].data[SensorData::legFR3];

    debugDataBuffer[1][0] = sensorDataBuffer.frame[0].data[SensorData::legFL1];
    debugDataBuffer[1][1] = sensorDataBuffer.frame[0].data[SensorData::legFL2];
    debugDataBuffer[1][2] = sensorDataBuffer.frame[0].data[SensorData::legFL3];
    debugDataBuffer[1][3] = sensorDataBuffer.frame[1].data[SensorData::legFL1];
    debugDataBuffer[1][4] = sensorDataBuffer.frame[1].data[SensorData::legFL2];
    debugDataBuffer[1][5] = sensorDataBuffer.frame[1].data[SensorData::legFL3];
    debugDataBuffer[1][6] = sensorDataBuffer.frame[2].data[SensorData::legFL1];
    debugDataBuffer[1][7] = sensorDataBuffer.frame[2].data[SensorData::legFL2];
    debugDataBuffer[1][8] = sensorDataBuffer.frame[2].data[SensorData::legFL3];
    debugDataBuffer[1][9] = sensorDataBuffer.frame[3].data[SensorData::legFL1];
    debugDataBuffer[1][10] = sensorDataBuffer.frame[3].data[SensorData::legFL2];
    debugDataBuffer[1][11] = sensorDataBuffer.frame[3].data[SensorData::legFL3];

    debugDataBuffer[2][0] = sensorDataBuffer.frame[0].data[SensorData::legHR1];
    debugDataBuffer[2][1] = sensorDataBuffer.frame[0].data[SensorData::legHR2];
    debugDataBuffer[2][2] = sensorDataBuffer.frame[0].data[SensorData::legHR3];
    debugDataBuffer[2][3] = sensorDataBuffer.frame[1].data[SensorData::legHR1];
    debugDataBuffer[2][4] = sensorDataBuffer.frame[1].data[SensorData::legHR2];
    debugDataBuffer[2][5] = sensorDataBuffer.frame[1].data[SensorData::legHR3];
    debugDataBuffer[2][6] = sensorDataBuffer.frame[2].data[SensorData::legHR1];
    debugDataBuffer[2][7] = sensorDataBuffer.frame[2].data[SensorData::legHR2];
    debugDataBuffer[2][8] = sensorDataBuffer.frame[2].data[SensorData::legHR3];
    debugDataBuffer[2][9] = sensorDataBuffer.frame[3].data[SensorData::legHR1];
    debugDataBuffer[2][10] = sensorDataBuffer.frame[3].data[SensorData::legHR2];
    debugDataBuffer[2][11] = sensorDataBuffer.frame[3].data[SensorData::legHR3];

    debugDataBuffer[3][0] = sensorDataBuffer.frame[0].data[SensorData::legHL1];
    debugDataBuffer[3][1] = sensorDataBuffer.frame[0].data[SensorData::legHL2];
    debugDataBuffer[3][2] = sensorDataBuffer.frame[0].data[SensorData::legHL3];
    debugDataBuffer[3][3] = sensorDataBuffer.frame[1].data[SensorData::legHL1];
    debugDataBuffer[3][4] = sensorDataBuffer.frame[1].data[SensorData::legHL2];
    debugDataBuffer[3][5] = sensorDataBuffer.frame[1].data[SensorData::legHL3];
    debugDataBuffer[3][6] = sensorDataBuffer.frame[2].data[SensorData::legHL1];
    debugDataBuffer[3][7] = sensorDataBuffer.frame[2].data[SensorData::legHL2];
    debugDataBuffer[3][8] = sensorDataBuffer.frame[2].data[SensorData::legHL3];
    debugDataBuffer[3][9] = sensorDataBuffer.frame[3].data[SensorData::legHL1];
    debugDataBuffer[3][10] = sensorDataBuffer.frame[3].data[SensorData::legHL2];
    debugDataBuffer[3][11] = sensorDataBuffer.frame[3].data[SensorData::legHL3];
  }

  // calculate position in space from joint angles
  Kinematics::legPositionFromJoints(Kinematics::fr,
    fromMicroRad(debugDataBuffer[0][3 * frameNumber + 0]),
    fromMicroRad(debugDataBuffer[0][3 * frameNumber + 1]),
    fromMicroRad(debugDataBuffer[0][3 * frameNumber + 2]),
    x, y, z);
  v[Leg_FR].x = x; v[Leg_FR].y = y - 67.2; v[Leg_FR].z = z;
  // set zero point to footZeroPosition
  v[Leg_FR] -= currentFIPSParameters->footZeroPosition[Leg_FR];
  // undo rotate
  real[Leg_FR].x = v[Leg_FR].x * cos(-c_angle[Leg_FR]) - v[Leg_FR].y * sin(-c_angle[Leg_FR]);
  real[Leg_FR].y = v[Leg_FR].x * sin(-c_angle[Leg_FR]) + v[Leg_FR].y * cos(-c_angle[Leg_FR]);
  real[Leg_FR].z = v[Leg_FR].z;
  // undo x-scale
  real[Leg_FR].x = real[Leg_FR].x / c_scale[Leg_FR];
  // transform back to foot position
  real[Leg_FR] += currentFIPSParameters->footZeroPosition[Leg_FR];
  //real[Leg_FR] += shoulderPosition[Leg_FR];

  Kinematics::legPositionFromJoints(Kinematics::fl,
    fromMicroRad(debugDataBuffer[1][3 * frameNumber + 0]),
    fromMicroRad(debugDataBuffer[1][3 * frameNumber + 1]),
    fromMicroRad(debugDataBuffer[1][3 * frameNumber + 2]),
    x, y, z);
  v[Leg_FL].x = x; v[Leg_FL].y = y + 67.2; v[Leg_FL].z = z;
  // set zero point to footZeroPosition
  v[Leg_FL] -= currentFIPSParameters->footZeroPosition[Leg_FL];
  // undo rotate
  real[Leg_FL].x = v[Leg_FL].x * cos(-c_angle[Leg_FL]) - v[Leg_FL].y * sin(-c_angle[Leg_FL]);
  real[Leg_FL].y = v[Leg_FL].x * sin(-c_angle[Leg_FL]) + v[Leg_FL].y * cos(-c_angle[Leg_FL]);
  real[Leg_FL].z = v[Leg_FL].z;
  // undo x-scale
  real[Leg_FL].x = real[Leg_FL].x / c_scale[Leg_FL];
  // transform back to foot position
  real[Leg_FL] += currentFIPSParameters->footZeroPosition[Leg_FL];
  //real[Leg_FL] += shoulderPosition[Leg_FL];

  Kinematics::legPositionFromJoints(Kinematics::hr,
    fromMicroRad(debugDataBuffer[2][3 * frameNumber + 0]),
    fromMicroRad(debugDataBuffer[2][3 * frameNumber + 1]),
    fromMicroRad(debugDataBuffer[2][3 * frameNumber + 2]),
    x, y, z);
  v[Leg_HR].x = x; v[Leg_HR].y = y - 67.2; v[Leg_HR].z = z;
  // set zero point to footZeroPosition
  v[Leg_HR] -= currentFIPSParameters->footZeroPosition[Leg_HR];
  // undo rotate
  real[Leg_HR].x = v[Leg_HR].x * cos(-c_angle[Leg_HR]) - v[Leg_HR].y * sin(-c_angle[Leg_HR]);
  real[Leg_HR].y = v[Leg_HR].x * sin(-c_angle[Leg_HR]) + v[Leg_HR].y * cos(-c_angle[Leg_HR]);
  real[Leg_HR].z = v[Leg_HR].z;
  // undo x-scale
  real[Leg_HR].x = real[Leg_HR].x / c_scale[Leg_HR];
  // transform back to foot position
  real[Leg_HR] += currentFIPSParameters->footZeroPosition[Leg_HR];
  //real[Leg_HR] += shoulderPosition[Leg_HR];

  Kinematics::legPositionFromJoints(Kinematics::hl,
    fromMicroRad(debugDataBuffer[3][3 * frameNumber + 0]),
    fromMicroRad(debugDataBuffer[3][3 * frameNumber + 1]),
    fromMicroRad(debugDataBuffer[3][3 * frameNumber + 2]),
    x, y, z);
  v[Leg_HL].x = x; v[Leg_HL].y = y + 67.2; v[Leg_HL].z = z;
  // set zero point to footZeroPosition
  v[Leg_HL] -= currentFIPSParameters->footZeroPosition[Leg_HL];
  // undo rotate
  real[Leg_HL].x = v[Leg_HL].x * cos(-c_angle[Leg_HL]) - v[Leg_HL].y * sin(-c_angle[Leg_HL]);
  real[Leg_HL].y = v[Leg_HL].x * sin(-c_angle[Leg_HL]) + v[Leg_HL].y * cos(-c_angle[Leg_HL]);
  real[Leg_HL].z = v[Leg_HL].z;
  // undo x-scale
  real[Leg_HL].x = real[Leg_HL].x / c_scale[Leg_HL];
  // transform back to foot position
  real[Leg_HL] += currentFIPSParameters->footZeroPosition[Leg_HL];
  //real[Leg_HL] += shoulderPosition[Leg_HL];

  // write in array
  debugDataReal[Leg_FR][currentStep] = real[Leg_FR];
  debugDataReal[Leg_FL][currentStep] = real[Leg_FL];
  debugDataReal[Leg_HR][currentStep] = real[Leg_HR];
  debugDataReal[Leg_HL][currentStep] = real[Leg_HL];

  debugDataIdeal[Leg_FR][currentStep] = ideal[Leg_FR];
  debugDataIdeal[Leg_FL][currentStep] = ideal[Leg_FL];
  debugDataIdeal[Leg_HR][currentStep] = ideal[Leg_HR];
  debugDataIdeal[Leg_HL][currentStep] = ideal[Leg_HL];

  debugRequest = false;
}

void GT2005WalkingEngine::readResampleData(double c_scale[4], double c_angle[4], double c_offset[4], JointData &j, Vector3<double> fp[4])
{
  double x, y, z;
  Vector3<double> v[4];
  Vector3<double> s[4];

  Vector3<double> real[4]; // corrected real vectors from sensor data
  Vector3<double> ideal[4]; // corrected ideal vectors from sensor data

  // define some hardcoded shoulder positions
  Vector3<double> shoulderPosition[4];
  shoulderPosition[Leg_FR].x = 0; shoulderPosition[Leg_FR].y = -67.2; shoulderPosition[Leg_FR].z = -19.5;
  shoulderPosition[Leg_FL].x = 0; shoulderPosition[Leg_FL].y = 67.2;  shoulderPosition[Leg_FL].z = -19.5;
  shoulderPosition[Leg_HR].x = -130;  shoulderPosition[Leg_HR].y = -67.2; shoulderPosition[Leg_HR].z = -19.5;
  shoulderPosition[Leg_HL].x = -130;  shoulderPosition[Leg_HL].y = 67.2;  shoulderPosition[Leg_HL].z = -19.5;

  // read forced angles
  // calculate position in space from joint angles
  Kinematics::legPositionFromJoints(Kinematics::fr,
    fromMicroRad(j.data[JointData::legFR1]) - bodyTilt,
    fromMicroRad(j.data[JointData::legFR2]),
    fromMicroRad(j.data[JointData::legFR3]),
    x, y, z);
  s[Leg_FR].x = x; s[Leg_FR].y = y - 67.2; s[Leg_FR].z = z;
  // set zero point to footZeroPosition
  s[Leg_FR] -= currentFIPSParameters->footZeroPosition[Leg_FR];
  // undo rotate
  ideal[Leg_FR].x = s[Leg_FR].x * cos(-c_angle[Leg_FR]) - s[Leg_FR].y * sin(-c_angle[Leg_FR]);
  ideal[Leg_FR].y = s[Leg_FR].x * sin(-c_angle[Leg_FR]) + s[Leg_FR].y * cos(-c_angle[Leg_FR]);
  ideal[Leg_FR].z = s[Leg_FR].z;
  // undo x-scale
  ideal[Leg_FR].x = ideal[Leg_FR].x / c_scale[Leg_FR];

  Kinematics::legPositionFromJoints(Kinematics::fl,
    fromMicroRad(j.data[JointData::legFL1]) - bodyTilt,
    fromMicroRad(j.data[JointData::legFL2]),
    fromMicroRad(j.data[JointData::legFL3]),
    x, y, z);
  s[Leg_FL].x = x; s[Leg_FL].y = y + 67.2; s[Leg_FL].z = z;
  // set zero point to footZeroPosition
  s[Leg_FL] -= currentFIPSParameters->footZeroPosition[Leg_FL];
  // undo rotate
  ideal[Leg_FL].x = s[Leg_FL].x * cos(-c_angle[Leg_FL]) - s[Leg_FL].y * sin(-c_angle[Leg_FL]);
  ideal[Leg_FL].y = s[Leg_FL].x * sin(-c_angle[Leg_FL]) + s[Leg_FL].y * cos(-c_angle[Leg_FL]);
  ideal[Leg_FL].z = s[Leg_FL].z;
  // undo x-scale
  ideal[Leg_FL].x = ideal[Leg_FL].x / c_scale[Leg_FL];

  Kinematics::legPositionFromJoints(Kinematics::hr,
    fromMicroRad(j.data[JointData::legHR1]) + bodyTilt,
    fromMicroRad(j.data[JointData::legHR2]),
    fromMicroRad(j.data[JointData::legHR3]),
    x, y, z);
  s[Leg_HR].x = x; s[Leg_HR].y = y - 67.2; s[Leg_HR].z = z;
  // set zero point to footZeroPosition
  s[Leg_HR] -= currentFIPSParameters->footZeroPosition[Leg_HR];
  // undo rotate
  ideal[Leg_HR].x = s[Leg_HR].x * cos(-c_angle[Leg_HR]) - s[Leg_HR].y * sin(-c_angle[Leg_HR]);
  ideal[Leg_HR].y = s[Leg_HR].x * sin(-c_angle[Leg_HR]) + s[Leg_HR].y * cos(-c_angle[Leg_HR]);
  ideal[Leg_HR].z = s[Leg_HR].z;
  // undo x-scale
  ideal[Leg_HR].x = ideal[Leg_HR].x / c_scale[Leg_HR];

  Kinematics::legPositionFromJoints(Kinematics::hl,
    fromMicroRad(j.data[JointData::legHL1]) + bodyTilt,
    fromMicroRad(j.data[JointData::legHL2]),
    fromMicroRad(j.data[JointData::legHL3]),
    x, y, z);
  s[Leg_HL].x = x; s[Leg_HL].y = y + 67.2; s[Leg_HL].z = z;
  // set zero point to footZeroPosition
  s[Leg_HL] -= currentFIPSParameters->footZeroPosition[Leg_HL];
  // undo rotate
  ideal[Leg_HL].x = s[Leg_HL].x * cos(-c_angle[Leg_HL]) - s[Leg_HL].y * sin(-c_angle[Leg_HL]);
  ideal[Leg_HL].y = s[Leg_HL].x * sin(-c_angle[Leg_HL]) + s[Leg_HL].y * cos(-c_angle[Leg_HL]);
  ideal[Leg_HL].z = s[Leg_HL].z;
  // undo x-scale
  ideal[Leg_HL].x = ideal[Leg_HL].x / c_scale[Leg_HL];

  // read real angles
  int frameNumber = currentStep % 4;

  if (frameNumber == 0)
  {
    debugDataBuffer[0][0] = sensorDataBuffer.frame[0].data[SensorData::legFR1];
    debugDataBuffer[0][1] = sensorDataBuffer.frame[0].data[SensorData::legFR2];
    debugDataBuffer[0][2] = sensorDataBuffer.frame[0].data[SensorData::legFR3];
    debugDataBuffer[0][3] = sensorDataBuffer.frame[1].data[SensorData::legFR1];
    debugDataBuffer[0][4] = sensorDataBuffer.frame[1].data[SensorData::legFR2];
    debugDataBuffer[0][5] = sensorDataBuffer.frame[1].data[SensorData::legFR3];
    debugDataBuffer[0][6] = sensorDataBuffer.frame[2].data[SensorData::legFR1];
    debugDataBuffer[0][7] = sensorDataBuffer.frame[2].data[SensorData::legFR2];
    debugDataBuffer[0][8] = sensorDataBuffer.frame[2].data[SensorData::legFR3];
    debugDataBuffer[0][9] = sensorDataBuffer.frame[3].data[SensorData::legFR1];
    debugDataBuffer[0][10] = sensorDataBuffer.frame[3].data[SensorData::legFR2];
    debugDataBuffer[0][11] = sensorDataBuffer.frame[3].data[SensorData::legFR3];

    debugDataBuffer[1][0] = sensorDataBuffer.frame[0].data[SensorData::legFL1];
    debugDataBuffer[1][1] = sensorDataBuffer.frame[0].data[SensorData::legFL2];
    debugDataBuffer[1][2] = sensorDataBuffer.frame[0].data[SensorData::legFL3];
    debugDataBuffer[1][3] = sensorDataBuffer.frame[1].data[SensorData::legFL1];
    debugDataBuffer[1][4] = sensorDataBuffer.frame[1].data[SensorData::legFL2];
    debugDataBuffer[1][5] = sensorDataBuffer.frame[1].data[SensorData::legFL3];
    debugDataBuffer[1][6] = sensorDataBuffer.frame[2].data[SensorData::legFL1];
    debugDataBuffer[1][7] = sensorDataBuffer.frame[2].data[SensorData::legFL2];
    debugDataBuffer[1][8] = sensorDataBuffer.frame[2].data[SensorData::legFL3];
    debugDataBuffer[1][9] = sensorDataBuffer.frame[3].data[SensorData::legFL1];
    debugDataBuffer[1][10] = sensorDataBuffer.frame[3].data[SensorData::legFL2];
    debugDataBuffer[1][11] = sensorDataBuffer.frame[3].data[SensorData::legFL3];

    debugDataBuffer[2][0] = sensorDataBuffer.frame[0].data[SensorData::legHR1];
    debugDataBuffer[2][1] = sensorDataBuffer.frame[0].data[SensorData::legHR2];
    debugDataBuffer[2][2] = sensorDataBuffer.frame[0].data[SensorData::legHR3];
    debugDataBuffer[2][3] = sensorDataBuffer.frame[1].data[SensorData::legHR1];
    debugDataBuffer[2][4] = sensorDataBuffer.frame[1].data[SensorData::legHR2];
    debugDataBuffer[2][5] = sensorDataBuffer.frame[1].data[SensorData::legHR3];
    debugDataBuffer[2][6] = sensorDataBuffer.frame[2].data[SensorData::legHR1];
    debugDataBuffer[2][7] = sensorDataBuffer.frame[2].data[SensorData::legHR2];
    debugDataBuffer[2][8] = sensorDataBuffer.frame[2].data[SensorData::legHR3];
    debugDataBuffer[2][9] = sensorDataBuffer.frame[3].data[SensorData::legHR1];
    debugDataBuffer[2][10] = sensorDataBuffer.frame[3].data[SensorData::legHR2];
    debugDataBuffer[2][11] = sensorDataBuffer.frame[3].data[SensorData::legHR3];

    debugDataBuffer[3][0] = sensorDataBuffer.frame[0].data[SensorData::legHL1];
    debugDataBuffer[3][1] = sensorDataBuffer.frame[0].data[SensorData::legHL2];
    debugDataBuffer[3][2] = sensorDataBuffer.frame[0].data[SensorData::legHL3];
    debugDataBuffer[3][3] = sensorDataBuffer.frame[1].data[SensorData::legHL1];
    debugDataBuffer[3][4] = sensorDataBuffer.frame[1].data[SensorData::legHL2];
    debugDataBuffer[3][5] = sensorDataBuffer.frame[1].data[SensorData::legHL3];
    debugDataBuffer[3][6] = sensorDataBuffer.frame[2].data[SensorData::legHL1];
    debugDataBuffer[3][7] = sensorDataBuffer.frame[2].data[SensorData::legHL2];
    debugDataBuffer[3][8] = sensorDataBuffer.frame[2].data[SensorData::legHL3];
    debugDataBuffer[3][9] = sensorDataBuffer.frame[3].data[SensorData::legHL1];
    debugDataBuffer[3][10] = sensorDataBuffer.frame[3].data[SensorData::legHL2];
    debugDataBuffer[3][11] = sensorDataBuffer.frame[3].data[SensorData::legHL3];
  }

  // calculate position in space from joint angles
  Kinematics::legPositionFromJoints(Kinematics::fr,
    fromMicroRad(debugDataBuffer[0][3 * frameNumber + 0]) - bodyTilt,
    fromMicroRad(debugDataBuffer[0][3 * frameNumber + 1]),
    fromMicroRad(debugDataBuffer[0][3 * frameNumber + 2]),
    x, y, z);
  v[Leg_FR].x = x; v[Leg_FR].y = y - 67.2; v[Leg_FR].z = z;
  // set zero point to footZeroPosition
  v[Leg_FR] -= currentFIPSParameters->footZeroPosition[Leg_FR];
  // undo rotate
  real[Leg_FR].x = v[Leg_FR].x * cos(-c_angle[Leg_FR]) - v[Leg_FR].y * sin(-c_angle[Leg_FR]);
  real[Leg_FR].y = v[Leg_FR].x * sin(-c_angle[Leg_FR]) + v[Leg_FR].y * cos(-c_angle[Leg_FR]);
  real[Leg_FR].z = v[Leg_FR].z;
  // undo x-scale
  real[Leg_FR].x = real[Leg_FR].x / c_scale[Leg_FR];

  Kinematics::legPositionFromJoints(Kinematics::fl,
    fromMicroRad(debugDataBuffer[1][3 * frameNumber + 0]) - bodyTilt,
    fromMicroRad(debugDataBuffer[1][3 * frameNumber + 1]),
    fromMicroRad(debugDataBuffer[1][3 * frameNumber + 2]),
    x, y, z);
  v[Leg_FL].x = x; v[Leg_FL].y = y + 67.2; v[Leg_FL].z = z;
  // set zero point to footZeroPosition
  v[Leg_FL] -= currentFIPSParameters->footZeroPosition[Leg_FL];
  // undo rotate
  real[Leg_FL].x = v[Leg_FL].x * cos(-c_angle[Leg_FL]) - v[Leg_FL].y * sin(-c_angle[Leg_FL]);
  real[Leg_FL].y = v[Leg_FL].x * sin(-c_angle[Leg_FL]) + v[Leg_FL].y * cos(-c_angle[Leg_FL]);
  real[Leg_FL].z = v[Leg_FL].z;
  // undo x-scale
  real[Leg_FL].x = real[Leg_FL].x / c_scale[Leg_FL];

  Kinematics::legPositionFromJoints(Kinematics::hr,
    fromMicroRad(debugDataBuffer[2][3 * frameNumber + 0]) + bodyTilt,
    fromMicroRad(debugDataBuffer[2][3 * frameNumber + 1]),
    fromMicroRad(debugDataBuffer[2][3 * frameNumber + 2]),
    x, y, z);
  v[Leg_HR].x = x; v[Leg_HR].y = y - 67.2; v[Leg_HR].z = z;
  // set zero point to footZeroPosition
  v[Leg_HR] -= currentFIPSParameters->footZeroPosition[Leg_HR];
  // undo rotate
  real[Leg_HR].x = v[Leg_HR].x * cos(-c_angle[Leg_HR]) - v[Leg_HR].y * sin(-c_angle[Leg_HR]);
  real[Leg_HR].y = v[Leg_HR].x * sin(-c_angle[Leg_HR]) + v[Leg_HR].y * cos(-c_angle[Leg_HR]);
  real[Leg_HR].z = v[Leg_HR].z;
  // undo x-scale
  real[Leg_HR].x = real[Leg_HR].x / c_scale[Leg_HR];

  Kinematics::legPositionFromJoints(Kinematics::hl,
    fromMicroRad(debugDataBuffer[3][3 * frameNumber + 0]) + bodyTilt,
    fromMicroRad(debugDataBuffer[3][3 * frameNumber + 1]),
    fromMicroRad(debugDataBuffer[3][3 * frameNumber + 2]),
    x, y, z);
  v[Leg_HL].x = x; v[Leg_HL].y = y + 67.2; v[Leg_HL].z = z;
  // set zero point to footZeroPosition
  v[Leg_HL] -= currentFIPSParameters->footZeroPosition[Leg_HL];
  // undo rotate
  real[Leg_HL].x = v[Leg_HL].x * cos(-c_angle[Leg_HL]) - v[Leg_HL].y * sin(-c_angle[Leg_HL]);
  real[Leg_HL].y = v[Leg_HL].x * sin(-c_angle[Leg_HL]) + v[Leg_HL].y * cos(-c_angle[Leg_HL]);
  real[Leg_HL].z = v[Leg_HL].z;
  // undo x-scale
  real[Leg_HL].x = real[Leg_HL].x / c_scale[Leg_HL];

  // write in array
  debugDataReal[Leg_FR][currentStep] = real[Leg_FR];
  debugDataReal[Leg_FL][currentStep] = real[Leg_FL];
  debugDataReal[Leg_HR][currentStep] = real[Leg_HR];
  debugDataReal[Leg_HL][currentStep] = real[Leg_HL];

  debugDataIdeal[Leg_FR][currentStep] = ideal[Leg_FR];
  debugDataIdeal[Leg_FL][currentStep] = ideal[Leg_FL];
  debugDataIdeal[Leg_HR][currentStep] = ideal[Leg_HR];
  debugDataIdeal[Leg_HL][currentStep] = ideal[Leg_HL];

  resampleRequest = false;
}

void GT2005WalkingEngine::streamDebugData()
{
  int i;

  // diese prozedur muss ich noch mal für das neue debugformat überarbeieten
  GT2005Polygon* idealPoly[4];
  GT2005Polygon* realPoly[4];
  for (i = 0; i < 4; i++)
  {
    idealPoly[i] = new GT2005Polygon(1, 0);
    realPoly[i] = new GT2005Polygon(1, 0);
  }

  idealPoly[Leg_FR]->numPoints = currentFIPSParameters->stepLength;
  idealPoly[Leg_FL]->numPoints = currentFIPSParameters->stepLength;
  idealPoly[Leg_HR]->numPoints = currentFIPSParameters->stepLength;
  idealPoly[Leg_HL]->numPoints = currentFIPSParameters->stepLength;
  realPoly[Leg_FR]->numPoints = currentFIPSParameters->stepLength;
  realPoly[Leg_FL]->numPoints = currentFIPSParameters->stepLength;
  realPoly[Leg_HR]->numPoints = currentFIPSParameters->stepLength;
  realPoly[Leg_HL]->numPoints = currentFIPSParameters->stepLength;

  // Get debug stream
  Out& out = getDebugOut().bin;

  // prepare data
  int dataID = 0;
  // real data
  for (i = 0; i < currentFIPSParameters->stepLength; i++)
  {
    idealPoly[Leg_FR]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataIdeal[Leg_FR][i]);
  }
  for (i = 0; i < currentFIPSParameters->stepLength; i++)
  {
    idealPoly[Leg_FL]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataIdeal[Leg_FL][i]);
  }
  for (i = 0; i < currentFIPSParameters->stepLength; i++)
  {
    idealPoly[Leg_HR]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataIdeal[Leg_HR][i]);
  }
  for (i = 0; i < currentFIPSParameters->stepLength; i++)
  {
    idealPoly[Leg_HL]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataIdeal[Leg_HL][i]);
  }
  // ideal data
  for (i = 0; i < currentFIPSParameters->stepLength; i++)
  {
    realPoly[Leg_FR]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataReal[Leg_FR][i]);
  }
  for (i = 0; i < currentFIPSParameters->stepLength; i++)
  {
    realPoly[Leg_FL]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataReal[Leg_FL][i]);
  }
  for (i = 0; i < currentFIPSParameters->stepLength; i++)
  {
    realPoly[Leg_HR]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataReal[Leg_HR][i]);
  }
  for (i = 0; i < currentFIPSParameters->stepLength; i++)
  {
    realPoly[Leg_HL]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataReal[Leg_HL][i]);
  }

  // stream data
  out << dataID;
  out << *idealPoly[Leg_FR];
  out << *idealPoly[Leg_FL];
  out << *idealPoly[Leg_HR];
  out << *idealPoly[Leg_HL];
  out << *realPoly[Leg_FR];
  out << *realPoly[Leg_FL];
  out << *realPoly[Leg_HR];
  out << *realPoly[Leg_HL];

  // Finish message
  getDebugOut().finishMessage(idGT2005DebugData);

  for (i = 0; i < 4; i++)
  {
    delete idealPoly[i];
    delete realPoly[i];
  }
}

---