Modules/WalkingEngine/GT2004ParameterSet.cpp

Go to the documentation of this file.

/**
* @file GT2004ParameterSet.cpp
* 
* Implementation of class GT2004Parameters and GT2004ParametersSet
*
* @author Uwe Düffert
*/

#include "GT2004ParameterSet.h"
#include "Modules/WalkingEngine/InvKinWalkingParameterSets.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Streams/OutStreams.h"

GT2004Parameters::GT2004Parameters(const InvKinWalkingParameters& invKinParam)
{
  int i;
  
  index=GT2004ParametersSet::numberOfParameters;
  requestedMotion=Pose2D(0,0,0);
  correctedMotion=Pose2D(0,invKinParam.maxStepSizeX,invKinParam.maxStepSizeY);
  
  foreHeight =invKinParam.foreHeight;
  foreWidth  =invKinParam.foreWidth;
  foreCenterX=invKinParam.foreCenterX;
  
  hindHeight =invKinParam.hindHeight;
  hindWidth  =invKinParam.hindWidth;
  hindCenterX=invKinParam.hindCenterX;
  
  foreFootLift=invKinParam.foreFootLift;
  hindFootLift=invKinParam.hindFootLift;
  
  foreFootTilt=invKinParam.foreFootTilt;
  hindFootTilt=invKinParam.hindFootTilt;
  
  stepLen=invKinParam.stepLen;
  
  switch(invKinParam.footMode)
  {
  case InvKinWalkingParameters::rectangle:
  case InvKinWalkingParameters::freeFormQuad:
    footMode=rectangle;
    break;
  case InvKinWalkingParameters::halfCircle:
  case InvKinWalkingParameters::circle:
  case InvKinWalkingParameters::rounded:
  case InvKinWalkingParameters::optimized:
  default:
    footMode=halfCircle;
    break;
  }
  
  for (i=0;i<2;i++)
  {
    groundPhase[i]=invKinParam.groundPhase[i];
liftPhase[i]=invKinParam.liftPhase[i];         //temp for BB support
loweringPhase[i]=invKinParam.loweringPhase[i]; //temp for BB support
  }
//bodyShiftX= invKinParam.bodyShiftX; //temp for BB support
//bodyShiftY= invKinParam.bodyShiftY; //temp for BB support
//bodyShiftOffset= invKinParam.bodyShiftOffset; //temp for BB support
  for (i=0;i<4;i++)
  {
    legPhase[i]=invKinParam.legPhase[i];
  }
  
  headTilt=invKinParam.headTilt;
  headPan=invKinParam.headPan;
  headRoll=invKinParam.headRoll;
  mouth=invKinParam.mouth;
  
  fitness=0;
}

bool GT2004Parameters::reportRealMotion(const Pose2D& real)
{
  bool result=true;
  correctedMotion.translation += (requestedMotion.translation-real.translation)/2;
  correctedMotion.rotation += (requestedMotion.rotation-real.rotation)/2;
  //allow no more than additional 23 percent on higher values:
  if ((fabs(correctedMotion.rotation)>1.23*fabs(requestedMotion.rotation))&&(fabs(correctedMotion.rotation)>1))
  {
    correctedMotion.rotation=1.23*requestedMotion.rotation;
    result=false;
  }
  if ((fabs(correctedMotion.translation.x)>1.23*fabs(requestedMotion.translation.x))&&(fabs(correctedMotion.translation.x)>150))
  {
    correctedMotion.translation.x=1.23*requestedMotion.translation.x;
    result=false;
  }
  if ((fabs(correctedMotion.translation.y)>1.23*fabs(requestedMotion.translation.y))&&(fabs(correctedMotion.translation.y)>150))
  {
    correctedMotion.translation.y=1.23*requestedMotion.translation.y;
    result=false;
  }
  return result;
}

In& operator>>(In& stream,GT2004Parameters& walkingParameters)
{
  stream >> walkingParameters.index;
  stream >> walkingParameters.requestedMotion;
  stream >> walkingParameters.correctedMotion;
  
  stream >> walkingParameters.foreHeight;
  stream >> walkingParameters.foreWidth; 
  stream >> walkingParameters.foreCenterX;
  stream >> walkingParameters.hindHeight; 
  stream >> walkingParameters.hindWidth;  
  stream >> walkingParameters.hindCenterX;
  stream >> walkingParameters.foreFootLift;
  stream >> walkingParameters.hindFootLift;
  stream >> walkingParameters.foreFootTilt;
  stream >> walkingParameters.hindFootTilt;
  stream >> walkingParameters.stepLen;
  int m;
  stream >> m;
  walkingParameters.footMode = (GT2004Parameters::FootMode)m;
  stream >> walkingParameters.groundPhase[0];
stream >> walkingParameters.liftPhase[0];  //temp for BB support
stream >> walkingParameters.loweringPhase[0];  //temp for BB support
  stream >> walkingParameters.groundPhase[1];
stream >> walkingParameters.liftPhase[1];  //temp for BB support
stream >> walkingParameters.loweringPhase[1];  //temp for BB support
//stream >> walkingParameters.bodyShiftX; //temp for BB support
//stream >> walkingParameters.bodyShiftY; //temp for BB support
//stream >> walkingParameters.bodyShiftOffset; //temp for BB support
  stream >> walkingParameters.legPhase[0];
  stream >> walkingParameters.legPhase[1];
  stream >> walkingParameters.legPhase[2];
  stream >> walkingParameters.legPhase[3];
  
  return stream;
}

Out& operator<<(Out& stream, const GT2004Parameters& walkingParameters)
{
  stream << walkingParameters.index;
  stream << walkingParameters.requestedMotion;
  stream << walkingParameters.correctedMotion;
  
  stream << walkingParameters.foreHeight;
  stream << walkingParameters.foreWidth; 
  stream << walkingParameters.foreCenterX;
  stream << walkingParameters.hindHeight; 
  stream << walkingParameters.hindWidth;  
  stream << walkingParameters.hindCenterX;
  stream << walkingParameters.foreFootLift;
  stream << walkingParameters.hindFootLift;
  stream << walkingParameters.foreFootTilt;
  stream << walkingParameters.hindFootTilt;
  stream << walkingParameters.stepLen;
  stream << (int)walkingParameters.footMode;
  stream << walkingParameters.groundPhase[0];
stream << walkingParameters.liftPhase[0];  //temp for BB support
stream << walkingParameters.loweringPhase[0];  //temp for BB support
  stream << walkingParameters.groundPhase[1];
stream << walkingParameters.liftPhase[1];  //temp for BB support
stream << walkingParameters.loweringPhase[1];  //temp for BB support
//stream << walkingParameters.bodyShiftX; //temp for BB support
//stream << walkingParameters.bodyShiftY; //temp for BB support
//stream << walkingParameters.bodyShiftOffset; //temp for BB support
  stream << walkingParameters.legPhase[0];
  stream << walkingParameters.legPhase[1];
  stream << walkingParameters.legPhase[2];
  stream << walkingParameters.legPhase[3];
  
  return stream;
}

GT2004ParametersSet::GT2004ParametersSet()
{
  lowRatioLowSpeed=&withWalk[0][0][0];
  lowRatioUpSpeed=&withWalk[0][0][0];
  upRatioLowSpeed=&withWalk[0][0][0];
  upRatioUpSpeed=&withWalk[0][0][0];
  lowSpeed=lowRatioLowSpeed;
  upSpeed=lowRatioUpSpeed;
  mergedParameters=lowSpeed;

  InvKinWalkingParameters invEvo("", 0, 76.5308, 78.1, 53.6242, 108.72, 76.95, -49.56, 5, 24, -0.25, 0.0450392, 1.1, 1.13782, 1.19384, 38, 43.0719, 100, 100, 1, 0.055, 77, 0.5, 0.06, 0.06, 0.5, 0.06, 0.06, 0, 0.5, 0.5, 0, 0.800923, 0, 0, 0);

  //BB original from walking.cfg:
  //InvKinWalkingParameters ers7fastforward("", 1, 77.3393, 78.4119, 61.2418, 133.442, 60.2821, -15.8993, 27.9704, 6.66998, -0.44709, 0.141395, 0.934017, 0.96636, 0.920326, 48.0381, 40.8256, 100, 100, 1, 0, 54, 0.436064, 0.0527674, -0.212436, 0.262644, 0.262884, -0.0536766, 0, 0.5, 0.4829755, 0.9829755, 0.0236576, 1.10329, 0.354185);
  //0 instead of 0.98 to allow interpolation:
  InvKinWalkingParameters ers7fastforward("", 1, 77.3393, 78.4119, 61.2418, 133.442, 60.2821, -15.8993, 27.9704, 6.66998, -0.44709, 0.141395, 0.934017, 0.96636, 0.920326, 48.0381, 40.8256, 100, 100, 1, 0, 54, 0.436064, 0.0527674, -0.212436, 0.262644, 0.262884, -0.0536766, 0, 0.5, 0.4829755, 0.0, 0.0236576, 1.10329, 0.354185);
  InvKinWalkingParameters ers7forward("", 1, 76.9667, 68.6102, 71.0797, 137.521, 63.6294, -11.0092, 31.4133, 9.099, -0.396573, 0.109536, 0.873517, 1.00862, 0.994123, 43.8919, 41.2512, 100, 100, 1, 0, 40, 0.384499, 0.0448997, -0.14587, 0.328341, 0.301144, -0.0379813, 0, 0.5, 0.52518189, 0.02518189, 0.169182, 0.844705, -0.527235);
  //InvKinWalkingParameters ers7backward("", 1, 72.2725, 62.5353, 45.1103, 127.76, 81.8199, -45.6655, -0.50958, 27.8999, -0.28543, 0.158594, 0.873871, 1.03927, 1.1683, 35.7206, 35.8633, 100, 100, 1, 0, 66, 0.555582, 0.189824, 0.12842, 0.478057, 0.151693, -0.0407527, 0, 0.5, 0.511624, 0.011624, -0.228749, -2.5242, 1.36218);
  //InvKinWalkingParameters msh7backward("", 0, 88.5057, 79.127, 43.2139, 101.905, 77.6874, -49.6692, 9.76973, 28.5799, -0.0724927, 0.0922574, 1.20455 0.843727, 1.44822, 34.8207, 38.8411, 100, 100, 1, 0.06, 61, 0.461105, 0.0361205, 0.0391534, 0.508829, 0.0532571, 0.0469326, 0, 0.5, 0.5, 0, 0, 0, 0.0337565);
  //negative tilts because GT2004 uses .abs()*tilt instead of .x*tilt:
  InvKinWalkingParameters ers7backward("", 1, 72.2725, 62.5353, 45.1103, 127.76, 81.8199, -45.6655, -0.50958, 27.8999, 0.28543, -0.158594, 0.873871, 1.03927, 1.1683, 35.7206, 35.8633, 100, 100, 1, 0, 66, 0.555582, 0.189824, 0.12842, 0.478057, 0.151693, -0.0407527, 0, 0.5, 0.511624, 0.011624, -0.228749, -2.5242, 1.36218);
  InvKinWalkingParameters msh7backward("", 0, 88.5057, 79.127, 43.2139, 101.905, 77.6874, -49.6692, 9.76973, 28.5799, +0.0724927, -0.0922574, 1.20455, 0.843727, 1.44822, 34.8207, 38.8411, 100, 100, 1, 0.06, 61, 0.461105, 0.0361205, 0.0391534, 0.508829, 0.0532571, 0.0469326, 0, 0.5, 0.5, 0, 0, 0, 0.0337565);

  GT2004Parameters msh7(0, Pose2D(0, 0, 0), 77.5836, 80.2702, 50.3536, 105.174, 81.5432, -52.0932, 5, 24, -0.25, 0.016, 64, 0, 0.49, 0.06, 0.06, 0.49, 0.06, 0.06, 0, 0.5, 0.5, 0);
  GT2004Parameters udTurn(0, Pose2D(0, 0, 0), 86.5308, 78.1, 33.6242, 113.72, 76.95, -29.56, 5, 24, -0.25, 0.0450392, 44, 0, 0.5, 0.06, 0.06, 0.5, 0.06, 0.06, 0, 0.5, 0.5, 0);
  
  for (int i=-3;i<=3;i++)
  {
    rotationOnly[3+i]=(abs(i)==3)?udTurn:msh7;//invEvo;
    rotationOnly[3+i].index=3+i;
    rotationOnly[3+i].requestedMotion=Pose2D(0.7*i,0,0);
    rotationOnly[3+i].correctedMotion=rotationOnly[3+i].requestedMotion;
  }
  rotationOnly[0].requestedMotion=Pose2D(-4.25,0,0);
  rotationOnly[0].correctedMotion=rotationOnly[0].requestedMotion;
  rotationOnly[6].requestedMotion=Pose2D(4.25,0,0);
  rotationOnly[6].correctedMotion=rotationOnly[6].requestedMotion;
  
  static const double ratios[]={-0.3*pi_2,-0.1*pi_2,0,0.1*pi_2,0.3*pi_2};
  for (int dir=-4;dir<4;dir++)
  {
    for (int rat=-2;rat<=2;rat++)
    {
      withWalk[2][rat+2][dir+4]=msh7;//invEvo;
      withWalk[2][rat+2][dir+4].index=7+5*8*2+8*(rat+2)+(dir+4);
      withWalk[2][rat+2][dir+4].requestedMotion=Pose2D(
        sin(ratios[rat+2])*0.9*2.7,
        cos(ratios[rat+2])*0.9*300*cos(dir*pi_4),
        cos(ratios[rat+2])*0.9*300*sin(dir*pi_4));
      withWalk[2][rat+2][dir+4].correctedMotion=withWalk[2][rat+2][dir+4].requestedMotion;
      
      withWalk[1][rat+2][dir+4]=msh7; //invEvo; //invQU;
      withWalk[1][rat+2][dir+4].index=7+5*8*1+8*(rat+2)+(dir+4);
      withWalk[1][rat+2][dir+4].requestedMotion=Pose2D(
        sin(ratios[rat+2])*0.5*2.7,
        cos(ratios[rat+2])*0.5*300*cos(dir*pi_4),
        cos(ratios[rat+2])*0.5*300*sin(dir*pi_4));
      withWalk[1][rat+2][dir+4].correctedMotion=withWalk[1][rat+2][dir+4].requestedMotion;
      
      withWalk[0][rat+2][dir+4]=msh7; //invEvo; //invQU;
      withWalk[0][rat+2][dir+4].index=7+5*8*0+8*(rat+2)+(dir+4);
      withWalk[0][rat+2][dir+4].requestedMotion=Pose2D(
        sin(ratios[rat+2])*0.2*2.7,
        cos(ratios[rat+2])*0.2*300*cos(dir*pi_4),
        cos(ratios[rat+2])*0.2*300*sin(dir*pi_4));
      withWalk[0][rat+2][dir+4].correctedMotion=withWalk[0][rat+2][dir+4].requestedMotion;
    }
  }
  
  withWalk[2][2][4]=msh7; //ers7fastforward;
  withWalk[2][2][4].index=7+5*8*2+8*2+4;
  withWalk[2][2][4].requestedMotion=Pose2D(0, 400,0);
  withWalk[2][2][4].correctedMotion=withWalk[2][2][4].requestedMotion;
  
  withWalk[1][2][4].requestedMotion=Pose2D(0, 250,0);
  withWalk[1][2][4].correctedMotion=withWalk[1][2][4].requestedMotion;
  
  withWalk[2][2][0]=msh7backward;
  withWalk[2][2][0].index=7+5*8*2+8*2+0;
  withWalk[2][2][0].requestedMotion=Pose2D(0, -294,0);
  withWalk[2][2][0].correctedMotion=withWalk[2][2][0].requestedMotion;
  
  mergedParameters=&rotationOnly[3];
  
  //if there is an parametersSet for this robotDesign, then load it:
  load();
}

void GT2004ParametersSet::mirrorLeftTurnToRightTurn()
{
  mirrorThis(few_lturn_0_fast);
  mirrorThis(few_lturn_min180_fast);
  mirrorThis(few_lturn_45_fast);
  mirrorThis(few_lturn_min135_fast);
  mirrorThis(few_lturn_90_fast);
  mirrorThis(few_lturn_min90_fast);
  mirrorThis(few_lturn_135_fast);
  mirrorThis(few_lturn_min45_fast);
  
  mirrorThis(few_lturn_0_med);
  mirrorThis(few_lturn_min180_med);
  mirrorThis(few_lturn_45_med);
  mirrorThis(few_lturn_min135_med);
  mirrorThis(few_lturn_90_med);
  mirrorThis(few_lturn_min90_med);
  mirrorThis(few_lturn_135_med);
  mirrorThis(few_lturn_min45_med);
  
  mirrorThis(few_lturn_0_slow);
  mirrorThis(few_lturn_min180_slow);
  mirrorThis(few_lturn_45_slow);
  mirrorThis(few_lturn_min135_slow);
  mirrorThis(few_lturn_90_slow);
  mirrorThis(few_lturn_min90_slow);
  mirrorThis(few_lturn_135_slow);
  mirrorThis(few_lturn_min45_slow);
  
  mirrorThis(med_lturn_0_fast);
  mirrorThis(med_lturn_min180_fast);
  mirrorThis(med_lturn_45_fast);
  mirrorThis(med_lturn_min135_fast);
  mirrorThis(med_lturn_90_fast);
  mirrorThis(med_lturn_min90_fast);
  mirrorThis(med_lturn_135_fast);
  mirrorThis(med_lturn_min45_fast);
  
  mirrorThis(med_lturn_0_med);
  mirrorThis(med_lturn_min180_med);
  mirrorThis(med_lturn_45_med);
  mirrorThis(med_lturn_min135_med);
  mirrorThis(med_lturn_90_med);
  mirrorThis(med_lturn_min90_med);
  mirrorThis(med_lturn_135_med);
  mirrorThis(med_lturn_min45_med);
  
  mirrorThis(med_lturn_0_slow);
  mirrorThis(med_lturn_min180_slow);
  mirrorThis(med_lturn_45_slow);
  mirrorThis(med_lturn_min135_slow);
  mirrorThis(med_lturn_90_slow);
  mirrorThis(med_lturn_min90_slow);
  mirrorThis(med_lturn_135_slow);
  mirrorThis(med_lturn_min45_slow);
  
  mirrorThis(much_lturn_slow);
  mirrorThis(much_lturn_med);
  mirrorThis(much_lturn_fast);
}

void GT2004ParametersSet::calculateMergedParameterSet(Pose2D& currentRequest)
{
  if ((currentRequest.rotation==0)&&(currentRequest.translation.abs()==0))
  {
    mergedParameters=&rotationOnly[3];
  }
  else
  {
    double rotationWalkRatio=atan2(currentRequest.rotation/2.7,currentRequest.translation.abs()/300)*2/pi;
    double movementStrength=sqrt(sqr(currentRequest.translation.abs()/300)+sqr(currentRequest.rotation/2.7));
    
    //determine in which turn/walk ratio segment we are:
    int lowerRatioIndex;
    double lowerRatioFactor;
    if (rotationWalkRatio<0)
    {
      if (rotationWalkRatio<-0.3)
      {
        lowerRatioIndex=0;
        lowerRatioFactor=(rotationWalkRatio+0.3)/(-1.0+0.3);
      }
      else
      {
        if (currentRequest.rotation<-0.1)
        {
          lowerRatioIndex=1;
          lowerRatioFactor=(rotationWalkRatio+0.1)/(-0.3+0.1);
        }
        else
        {
          lowerRatioIndex=2;
          lowerRatioFactor=rotationWalkRatio/(-0.1);
        }
      }
    }
    else
    {
      if (rotationWalkRatio<0.3)
      {
        if (rotationWalkRatio<0.1)
        {
          lowerRatioIndex=3;
          lowerRatioFactor=(0.1-rotationWalkRatio)/(0.1);
        }
        else
        {
          lowerRatioIndex=4;
          lowerRatioFactor=(0.3-rotationWalkRatio)/(0.3-0.1);
        }
      }
      else
      {
        lowerRatioIndex=5;
        lowerRatioFactor=(1.0-rotationWalkRatio)/(1.0-0.3);
      }
    }
    
    //determine in which walk direction segment we are
    double direction=atan2(currentRequest.translation.y,currentRequest.translation.x);
    if (direction==pi) { direction=-pi; }
    int lowerDirectionIndex=(int)((direction/pi+1)*4);
    double lowerDirectionFactor= 1.0-((direction/pi+1)*4-lowerDirectionIndex);
    
    //calculate the maximum movement strength into the requested direction with requested turn/walk ratio
    double lowerRatioMaxStrength=(lowerRatioIndex==0)?fabs(rotationOnly[0].requestedMotion.rotation)/2.7:
    sqrt(
      sqr(withWalk[2][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
      sqr(withWalk[2][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
      sqrt(
      sqr(withWalk[2][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
      sqr(withWalk[2][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
    
    double upperRatioMaxStrength=(lowerRatioIndex>=5)?fabs(rotationOnly[6].requestedMotion.rotation)/2.7:
    sqrt(
      sqr(withWalk[2][lowerRatioIndex][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
      sqr(withWalk[2][lowerRatioIndex][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
      sqrt(
      sqr(withWalk[2][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
      sqr(withWalk[2][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
    double maxStrength=lowerRatioMaxStrength*lowerRatioFactor + upperRatioMaxStrength*(1.0-lowerRatioFactor);
    
    //determine in which speed segment we are
    int lowerSpeedIndex;
    double lowerSpeedFactor;
    if (movementStrength>=maxStrength)
    {
      //restrict requested motion to maximum strength with that direction and turn/walk ratio
      currentRequest.translation *= maxStrength/movementStrength;
      currentRequest.rotation *= maxStrength/movementStrength;
      movementStrength=maxStrength;
      lowerSpeedIndex=3;
      lowerSpeedFactor=1;
    }
    else
    {
      //calculation of medStrength
      double lowerRatioMedStrength=(lowerRatioIndex==0)?fabs(rotationOnly[1].requestedMotion.rotation)/2.7:
      sqrt(
        sqr(withWalk[1][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
        sqr(withWalk[1][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
        sqrt(
        sqr(withWalk[1][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
        sqr(withWalk[1][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
      
      double upperRatioMedStrength=(lowerRatioIndex>=5)?fabs(rotationOnly[5].requestedMotion.rotation)/2.7:
      sqrt(
        sqr(withWalk[1][lowerRatioIndex][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
        sqr(withWalk[1][lowerRatioIndex][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
        sqrt(
        sqr(withWalk[1][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
        sqr(withWalk[1][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
      double medStrength=lowerRatioMedStrength*lowerRatioFactor + upperRatioMedStrength*(1.0-lowerRatioFactor);
      
      if (movementStrength>=medStrength)
      {
        lowerSpeedIndex=2;
        lowerSpeedFactor=(maxStrength-movementStrength)/(maxStrength-medStrength);
      }
      else
      {
        //calculation of minStrength
        double lowerRatioMinStrength=(lowerRatioIndex==0)?fabs(rotationOnly[2].requestedMotion.rotation)/2.7:
        sqrt(
          sqr(withWalk[0][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
          sqr(withWalk[0][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
          sqrt(
          sqr(withWalk[0][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
          sqr(withWalk[0][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
        
        double upperRatioMinStrength=(lowerRatioIndex>=5)?fabs(rotationOnly[4].requestedMotion.rotation)/2.7:
        sqrt(
          sqr(withWalk[0][lowerRatioIndex][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
          sqr(withWalk[0][lowerRatioIndex][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
          sqrt(
          sqr(withWalk[0][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
          sqr(withWalk[0][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
        double minStrength=lowerRatioMinStrength*lowerRatioFactor + upperRatioMinStrength*(1.0-lowerRatioFactor);
        if (movementStrength>=minStrength)
        {
          lowerSpeedIndex=1;
          lowerSpeedFactor=(medStrength-movementStrength)/(medStrength-minStrength);
        }
        else
        {
          lowerSpeedIndex=0;
          lowerSpeedFactor=(minStrength-movementStrength)/(minStrength-0);
        }
      }
    }
    
    //calculate merged parameter set(s) with lower/upperRatio/Direction/SpeedIndex/Factor
    if (lowerRatioFactor>0)
    {
      //calculate merged parameter set for lower ratio and lower speed
      if (lowerSpeedIndex==0)
      {
        lowRatioLowSpeed=&rotationOnly[3];
      }
      else if (lowerRatioIndex==0)
      {
        //only turn right
        lowRatioLowSpeed=&rotationOnly[3-lowerSpeedIndex];
      }
      else
      {
        if (lowerDirectionFactor==1)
        {
          lowRatioLowSpeed=&withWalk[lowerSpeedIndex-1][lowerRatioIndex-1][lowerDirectionIndex];
        }
        else
        {
          lowRatioLowSpeed=&llBuf;
          lowRatioLowSpeed->interpolate(
            withWalk[lowerSpeedIndex-1][lowerRatioIndex-1][lowerDirectionIndex],
            withWalk[lowerSpeedIndex-1][lowerRatioIndex-1][(lowerDirectionIndex+1)%8],
            lowerDirectionFactor);
        }
      }
      
      //calculate merged parameter set for lower ratio and upper speed
      if (lowerSpeedIndex==3)
      {
        lowRatioUpSpeed=lowRatioLowSpeed;
      }
      else if (lowerRatioIndex==0)
      {
        lowRatioUpSpeed=&rotationOnly[3-min(lowerSpeedIndex+1,3)];
      }
      else
      {
        if (lowerDirectionFactor==1)
        {
          lowRatioUpSpeed=&withWalk[lowerSpeedIndex][lowerRatioIndex-1][lowerDirectionIndex];
        }
        else
        {
          lowRatioUpSpeed=&luBuf;
          lowRatioUpSpeed->interpolate(
            withWalk[lowerSpeedIndex][lowerRatioIndex-1][lowerDirectionIndex],
            withWalk[lowerSpeedIndex][lowerRatioIndex-1][(lowerDirectionIndex+1)%8],
            lowerDirectionFactor);
        }
      }
    }
    
    if (lowerRatioFactor<1)
    {
      //calculate merged parameter set for upper ratio and lower speed
      if (lowerRatioIndex>=5)
      {
        //only turn left
        upRatioLowSpeed=&rotationOnly[3+lowerSpeedIndex];
      }
      else if (lowerSpeedIndex==0)
      {
        upRatioLowSpeed=&rotationOnly[3];
      }
      else
      {
        if (lowerDirectionFactor==1)
        {
          upRatioLowSpeed=&withWalk[lowerSpeedIndex-1][lowerRatioIndex][lowerDirectionIndex];
        }
        else
        {
          upRatioLowSpeed=&ulBuf;
          upRatioLowSpeed->interpolate(
            withWalk[lowerSpeedIndex-1][lowerRatioIndex][lowerDirectionIndex],
            withWalk[lowerSpeedIndex-1][lowerRatioIndex][(lowerDirectionIndex+1)%8],
            lowerDirectionFactor);
        }
      }
      
      //calculate merged parameter set for upper ratio and upper speed
      if (lowerRatioIndex>=5)
      {
        //only turn left
        upRatioUpSpeed=&rotationOnly[3+min(lowerSpeedIndex+1,3)];
      }
      else if (lowerSpeedIndex==3)
      {
        upRatioUpSpeed=upRatioLowSpeed;
      }
      else
      {
        if (lowerDirectionFactor==1)
        {
          upRatioUpSpeed=&withWalk[lowerSpeedIndex][lowerRatioIndex][lowerDirectionIndex];
        }
        else
        {
          upRatioUpSpeed=&uuBuf;
          upRatioUpSpeed->interpolate(
            withWalk[lowerSpeedIndex][lowerRatioIndex][lowerDirectionIndex],
            withWalk[lowerSpeedIndex][lowerRatioIndex][(lowerDirectionIndex+1)%8],
            lowerDirectionFactor);
        }
      }
    }
    
    //calculate merged parameter sets for lower and upper speed
    if (lowerRatioFactor==1)
    {
      lowSpeed=lowRatioLowSpeed;
      upSpeed=lowRatioUpSpeed;
    }
    else if (lowerRatioFactor==0)
    {
      lowSpeed=upRatioLowSpeed;
      upSpeed=upRatioUpSpeed;
    }
    else
    {
      lowSpeed=&lBuf;
      lowSpeed->interpolate(*lowRatioLowSpeed,*upRatioLowSpeed,lowerRatioFactor);
      if (lowerSpeedFactor==1)
      {
        upSpeed=lowSpeed;
      }
      else
      {
        upSpeed=&uBuf;
        upSpeed->interpolate(*lowRatioUpSpeed,*upRatioUpSpeed,lowerRatioFactor);
      }
    }
    
    //calculate finally merged parameter set
    if (lowerSpeedFactor==1)
    {
      mergedParameters=lowSpeed;
    }
    else
    {
      mergedParameters=&mBuf;
      mergedParameters->interpolate(*lowSpeed,*upSpeed,lowerSpeedFactor);
    }
  }
}

GT2004Parameters* GT2004ParametersSet::getParameters(int index)
{
  if (index<7)
  {
    if (index<0){ index=0; }
    return &rotationOnly[index];
  }
  else
  {
    if (index>=7+3*5*8){ index=7+3*5*8-1; }
    int direction=(index-7)%8;
    int ratio=((index-7)/8)%5;
    int speed=(index-7)/40;
    return &withWalk[speed][ratio][direction];
  }
}
bool GT2004ParametersSet::isMaxSpeedIndex(int index)
{
  if (index<7)
  {
    if (index<0){ index=0; }
    return ((index==0)||(index==6));
  }
  else
  {
    if (index>=7+3*5*8){ index=7+3*5*8-1; }
    int speed=(index-7)/40;
    return (speed==2);
  }
}

char* GT2004ParametersSet::getIndexString(int index)
{
  switch ((IndexName)index)
  {
  case much_lturn_fast: return "much_lturn_fast"; break;
  case much_lturn_med: return "much_lturn_med"; break;
  case much_lturn_slow: return "much_lturn_slow"; break;
  case stand: return "stand"; break;
  case much_rturn_slow: return "much_rturn_slow"; break;
  case much_rturn_med: return "much_rturn_med"; break;
  case much_rturn_fast: return "much_rturn_fast"; break;
  case med_lturn_min180_slow: return "med_lturn_min180_slow"; break;
  case med_lturn_min135_slow: return "med_lturn_min135_slow"; break;
  case med_lturn_min90_slow: return "med_lturn_min90_slow"; break;
  case med_lturn_min45_slow: return "med_lturn_min45_slow"; break;
  case med_lturn_0_slow: return "med_lturn_0_slow"; break;
  case med_lturn_45_slow: return "med_lturn_45_slow"; break;
  case med_lturn_90_slow: return "med_lturn_90_slow"; break;
  case med_lturn_135_slow: return "med_lturn_135_slow"; break;
  case few_lturn_min180_slow: return "few_lturn_min180_slow"; break;
  case few_lturn_min135_slow: return "few_lturn_min135_slow"; break;
  case few_lturn_min90_slow: return "few_lturn_min90_slow"; break;
  case few_lturn_min45_slow: return "few_lturn_min45_slow"; break;
  case few_lturn_0_slow: return "few_lturn_0_slow"; break;
  case few_lturn_45_slow: return "few_lturn_45_slow"; break;
  case few_lturn_90_slow: return "few_lturn_90_slow"; break;
  case few_lturn_135_slow: return "few_lturn_135_slow"; break;
  case no_turn_min180_slow: return "no_turn_min180_slow"; break;
  case no_turn_min135_slow: return "no_turn_min135_slow"; break;
  case no_turn_min90_slow: return "no_turn_min90_slow"; break;
  case no_turn_min45_slow: return "no_turn_min45_slow"; break;
  case no_turn_0_slow: return "no_turn_0_slow"; break;
  case no_turn_45_slow: return "no_turn_45_slow"; break;
  case no_turn_90_slow: return "no_turn_90_slow"; break;
  case no_turn_135_slow: return "no_turn_135_slow"; break;
  case few_rturn_min180_slow: return "few_rturn_min180_slow"; break;
  case few_rturn_min135_slow: return "few_rturn_min135_slow"; break;
  case few_rturn_min90_slow: return "few_rturn_min90_slow"; break;
  case few_rturn_min45_slow: return "few_rturn_min45_slow"; break;
  case few_rturn_0_slow: return "few_rturn_0_slow"; break;
  case few_rturn_45_slow: return "few_rturn_45_slow"; break;
  case few_rturn_90_slow: return "few_rturn_90_slow"; break;
  case few_rturn_135_slow: return "few_rturn_135_slow"; break;
  case med_rturn_min180_slow: return "med_rturn_min180_slow"; break;
  case med_rturn_min135_slow: return "med_rturn_min135_slow"; break;
  case med_rturn_min90_slow: return "med_rturn_min90_slow"; break;
  case med_rturn_min45_slow: return "med_rturn_min45_slow"; break;
  case med_rturn_0_slow: return "med_rturn_0_slow"; break;
  case med_rturn_45_slow: return "med_rturn_45_slow"; break;
  case med_rturn_90_slow: return "med_rturn_90_slow"; break;
  case med_rturn_135_slow: return "med_rturn_135_slow"; break;
  case med_lturn_min180_med: return "med_lturn_min180_med"; break;
  case med_lturn_min135_med: return "med_lturn_min135_med"; break;
  case med_lturn_min90_med: return "med_lturn_min90_med"; break;
  case med_lturn_min45_med: return "med_lturn_min45_med"; break;
  case med_lturn_0_med: return "med_lturn_0_med"; break;
  case med_lturn_45_med: return "med_lturn_45_med"; break;
  case med_lturn_90_med: return "med_lturn_90_med"; break;
  case med_lturn_135_med: return "med_lturn_135_med"; break;
  case few_lturn_min180_med: return "few_lturn_min180_med"; break;
  case few_lturn_min135_med: return "few_lturn_min135_med"; break;
  case few_lturn_min90_med: return "few_lturn_min90_med"; break;
  case few_lturn_min45_med: return "few_lturn_min45_med"; break;
  case few_lturn_0_med: return "few_lturn_0_med"; break;
  case few_lturn_45_med: return "few_lturn_45_med"; break;
  case few_lturn_90_med: return "few_lturn_90_med"; break;
  case few_lturn_135_med: return "few_lturn_135_med"; break;
  case no_turn_min180_med: return "no_turn_min180_med"; break;
  case no_turn_min135_med: return "no_turn_min135_med"; break;
  case no_turn_min90_med: return "no_turn_min90_med"; break;
  case no_turn_min45_med: return "no_turn_min45_med"; break;
  case no_turn_0_med: return "no_turn_0_med"; break;
  case no_turn_45_med: return "no_turn_45_med"; break;
  case no_turn_90_med: return "no_turn_90_med"; break;
  case no_turn_135_med: return "no_turn_135_med"; break;
  case few_rturn_min180_med: return "few_rturn_min180_med"; break;
  case few_rturn_min135_med: return "few_rturn_min135_med"; break;
  case few_rturn_min90_med: return "few_rturn_min90_med"; break;
  case few_rturn_min45_med: return "few_rturn_min45_med"; break;
  case few_rturn_0_med: return "few_rturn_0_med"; break;
  case few_rturn_45_med: return "few_rturn_45_med"; break;
  case few_rturn_90_med: return "few_rturn_90_med"; break;
  case few_rturn_135_med: return "few_rturn_135_med"; break;
  case med_rturn_min180_med: return "med_rturn_min180_med"; break;
  case med_rturn_min135_med: return "med_rturn_min135_med"; break;
  case med_rturn_min90_med: return "med_rturn_min90_med"; break;
  case med_rturn_min45_med: return "med_rturn_min45_med"; break;
  case med_rturn_0_med: return "med_rturn_0_med"; break;
  case med_rturn_45_med: return "med_rturn_45_med"; break;
  case med_rturn_90_med: return "med_rturn_90_med"; break;
  case med_rturn_135_med: return "med_rturn_135_med"; break;
  case med_lturn_min180_fast: return "med_lturn_min180_fast"; break;
  case med_lturn_min135_fast: return "med_lturn_min135_fast"; break;
  case med_lturn_min90_fast: return "med_lturn_min90_fast"; break;
  case med_lturn_min45_fast: return "med_lturn_min45_fast"; break;
  case med_lturn_0_fast: return "med_lturn_0_fast"; break;
  case med_lturn_45_fast: return "med_lturn_45_fast"; break;
  case med_lturn_90_fast: return "med_lturn_90_fast"; break;
  case med_lturn_135_fast: return "med_lturn_135_fast"; break;
  case few_lturn_min180_fast: return "few_lturn_min180_fast"; break;
  case few_lturn_min135_fast: return "few_lturn_min135_fast"; break;
  case few_lturn_min90_fast: return "few_lturn_min90_fast"; break;
  case few_lturn_min45_fast: return "few_lturn_min45_fast"; break;
  case few_lturn_0_fast: return "few_lturn_0_fast"; break;
  case few_lturn_45_fast: return "few_lturn_45_fast"; break;
  case few_lturn_90_fast: return "few_lturn_90_fast"; break;
  case few_lturn_135_fast: return "few_lturn_135_fast"; break;
  case no_turn_min180_fast: return "no_turn_min180_fast"; break;
  case no_turn_min135_fast: return "no_turn_min135_fast"; break;
  case no_turn_min90_fast: return "no_turn_min90_fast"; break;
  case no_turn_min45_fast: return "no_turn_min45_fast"; break;
  case no_turn_0_fast: return "no_turn_0_fast"; break;
  case no_turn_45_fast: return "no_turn_45_fast"; break;
  case no_turn_90_fast: return "no_turn_90_fast"; break;
  case no_turn_135_fast: return "no_turn_135_fast"; break;
  case few_rturn_min180_fast: return "few_rturn_min180_fast"; break;
  case few_rturn_min135_fast: return "few_rturn_min135_fast"; break;
  case few_rturn_min90_fast: return "few_rturn_min90_fast"; break;
  case few_rturn_min45_fast: return "few_rturn_min45_fast"; break;
  case few_rturn_0_fast: return "few_rturn_0_fast"; break;
  case few_rturn_45_fast: return "few_rturn_45_fast"; break;
  case few_rturn_90_fast: return "few_rturn_90_fast"; break;
  case few_rturn_135_fast: return "few_rturn_135_fast"; break;
  case med_rturn_min180_fast: return "med_rturn_min180_fast"; break;
  case med_rturn_min135_fast: return "med_rturn_min135_fast"; break;
  case med_rturn_min90_fast: return "med_rturn_min90_fast"; break;
  case med_rturn_min45_fast: return "med_rturn_min45_fast"; break;
  case med_rturn_0_fast: return "med_rturn_0_fast"; break;
  case med_rturn_45_fast: return "med_rturn_45_fast"; break;
  case med_rturn_90_fast: return "med_rturn_90_fast"; break;
  case med_rturn_135_fast: return "med_rturn_135_fast"; break;
  default: return "undefined";
  }
}

int GT2004ParametersSet::getIndexOfMirror(int index)
{
  switch ((IndexName)index)
  {
    //turn only:
  case much_lturn_fast: return (int)much_rturn_fast; break;
  case much_lturn_med: return (int)much_rturn_med; break;
  case much_lturn_slow: return (int)much_rturn_slow; break;
  case much_rturn_slow: return (int)much_lturn_slow; break;
  case much_rturn_med: return (int)much_lturn_med; break;
  case much_rturn_fast: return (int)much_lturn_fast; break;
    //180:
  case med_lturn_min180_slow: return (int)med_rturn_min180_slow; break;
  case few_lturn_min180_slow: return (int)few_rturn_min180_slow; break;
  case few_rturn_min180_slow: return (int)few_lturn_min180_slow; break;
  case med_rturn_min180_slow: return (int)med_lturn_min180_slow; break;
  case med_lturn_min180_med: return (int)med_rturn_min180_med; break;
  case few_lturn_min180_med: return (int)few_rturn_min180_med; break;
  case few_rturn_min180_med: return (int)few_lturn_min180_med; break;
  case med_rturn_min180_med: return (int)med_lturn_min180_med; break;
  case med_lturn_min180_fast: return (int)med_rturn_min180_fast; break;
  case few_lturn_min180_fast: return (int)few_rturn_min180_fast; break;
  case few_rturn_min180_fast: return (int)few_lturn_min180_fast; break;
  case med_rturn_min180_fast: return (int)med_lturn_min180_fast; break;
    //0:
  case med_lturn_0_slow: return (int)med_rturn_0_slow; break;
  case few_lturn_0_slow: return (int)few_rturn_0_slow; break;
  case few_rturn_0_slow: return (int)few_lturn_0_slow; break;
  case med_rturn_0_slow: return (int)med_lturn_0_slow; break;
  case med_lturn_0_med: return (int)med_rturn_0_med; break;
  case few_lturn_0_med: return (int)few_rturn_0_med; break;
  case few_rturn_0_med: return (int)few_lturn_0_med; break;
  case med_rturn_0_med: return (int)med_lturn_0_med; break;
  case med_lturn_0_fast: return (int)med_rturn_0_fast; break;
  case few_lturn_0_fast: return (int)few_rturn_0_fast; break;
  case few_rturn_0_fast: return (int)few_lturn_0_fast; break;
  case med_rturn_0_fast: return (int)med_lturn_0_fast; break;
    //+-135 slow:
  case med_lturn_min135_slow: return (int)med_rturn_135_slow; break;
  case few_lturn_min135_slow: return (int)few_rturn_135_slow; break;
  case no_turn_min135_slow: return (int)no_turn_135_slow; break;
  case few_rturn_min135_slow: return (int)few_lturn_135_slow; break;
  case med_rturn_min135_slow: return (int)med_lturn_135_slow; break;
  case med_lturn_135_slow: return (int)med_rturn_min135_slow; break;
  case few_lturn_135_slow: return (int)few_rturn_min135_slow; break;
  case no_turn_135_slow: return (int)no_turn_min135_slow; break;
  case few_rturn_135_slow: return (int)few_lturn_min135_slow; break;
  case med_rturn_135_slow: return (int)med_lturn_min135_slow; break;
    //+-90 slow:
  case med_lturn_min90_slow: return (int)med_rturn_90_slow; break;
  case few_lturn_min90_slow: return (int)few_rturn_90_slow; break;
  case no_turn_min90_slow: return (int)no_turn_90_slow; break;
  case few_rturn_min90_slow: return (int)few_lturn_90_slow; break;
  case med_rturn_min90_slow: return (int)med_lturn_90_slow; break;
  case med_lturn_90_slow: return (int)med_rturn_min90_slow; break;
  case few_lturn_90_slow: return (int)few_rturn_min90_slow; break;
  case no_turn_90_slow: return (int)no_turn_min90_slow; break;
  case few_rturn_90_slow: return (int)few_lturn_min90_slow; break;
  case med_rturn_90_slow: return (int)med_lturn_min90_slow; break;
    //+-45 slow:
  case med_lturn_min45_slow: return (int)med_rturn_45_slow; break;
  case few_lturn_min45_slow: return (int)few_rturn_45_slow; break;
  case no_turn_min45_slow: return (int)no_turn_45_slow; break;
  case few_rturn_min45_slow: return (int)few_lturn_45_slow; break;
  case med_rturn_min45_slow: return (int)med_lturn_45_slow; break;
  case med_lturn_45_slow: return (int)med_rturn_min45_slow; break;
  case few_lturn_45_slow: return (int)few_rturn_min45_slow; break;
  case no_turn_45_slow: return (int)no_turn_min45_slow; break;
  case few_rturn_45_slow: return (int)few_lturn_min45_slow; break;
  case med_rturn_45_slow: return (int)med_lturn_min45_slow; break;
    //+-135 med:
  case med_lturn_min135_med: return (int)med_rturn_135_med; break;
  case few_lturn_min135_med: return (int)few_rturn_135_med; break;
  case no_turn_min135_med: return (int)no_turn_135_med; break;
  case few_rturn_min135_med: return (int)few_lturn_135_med; break;
  case med_rturn_min135_med: return (int)med_lturn_135_med; break;
  case med_lturn_135_med: return (int)med_rturn_min135_med; break;
  case few_lturn_135_med: return (int)few_rturn_min135_med; break;
  case no_turn_135_med: return (int)no_turn_min135_med; break;
  case few_rturn_135_med: return (int)few_lturn_min135_med; break;
  case med_rturn_135_med: return (int)med_lturn_min135_med; break;
    //+-90 med:
  case med_lturn_min90_med: return (int)med_rturn_90_med; break;
  case few_lturn_min90_med: return (int)few_rturn_90_med; break;
  case no_turn_min90_med: return (int)no_turn_90_med; break;
  case few_rturn_min90_med: return (int)few_lturn_90_med; break;
  case med_rturn_min90_med: return (int)med_lturn_90_med; break;
  case med_lturn_90_med: return (int)med_rturn_min90_med; break;
  case few_lturn_90_med: return (int)few_rturn_min90_med; break;
  case no_turn_90_med: return (int)no_turn_min90_med; break;
  case few_rturn_90_med: return (int)few_lturn_min90_med; break;
  case med_rturn_90_med: return (int)med_lturn_min90_med; break;
    //+-45 med:
  case med_lturn_min45_med: return (int)med_rturn_45_med; break;
  case few_lturn_min45_med: return (int)few_rturn_45_med; break;
  case no_turn_min45_med: return (int)no_turn_45_med; break;
  case few_rturn_min45_med: return (int)few_lturn_45_med; break;
  case med_rturn_min45_med: return (int)med_lturn_45_med; break;
  case med_lturn_45_med: return (int)med_rturn_min45_med; break;
  case few_lturn_45_med: return (int)few_rturn_min45_med; break;
  case no_turn_45_med: return (int)no_turn_min45_med; break;
  case few_rturn_45_med: return (int)few_lturn_min45_med; break;
  case med_rturn_45_med: return (int)med_lturn_min45_med; break;
    //+-135 fast:
  case med_lturn_min135_fast: return (int)med_rturn_135_fast; break;
  case few_lturn_min135_fast: return (int)few_rturn_135_fast; break;
  case no_turn_min135_fast: return (int)no_turn_135_fast; break;
  case few_rturn_min135_fast: return (int)few_lturn_135_fast; break;
  case med_rturn_min135_fast: return (int)med_lturn_135_fast; break;
  case med_lturn_135_fast: return (int)med_rturn_min135_fast; break;
  case few_lturn_135_fast: return (int)few_rturn_min135_fast; break;
  case no_turn_135_fast: return (int)no_turn_min135_fast; break;
  case few_rturn_135_fast: return (int)few_lturn_min135_fast; break;
  case med_rturn_135_fast: return (int)med_lturn_min135_fast; break;
    //+-90 fast:
  case med_lturn_min90_fast: return (int)med_rturn_90_fast; break;
  case few_lturn_min90_fast: return (int)few_rturn_90_fast; break;
  case no_turn_min90_fast: return (int)no_turn_90_fast; break;
  case few_rturn_min90_fast: return (int)few_lturn_90_fast; break;
  case med_rturn_min90_fast: return (int)med_lturn_90_fast; break;
  case med_lturn_90_fast: return (int)med_rturn_min90_fast; break;
  case few_lturn_90_fast: return (int)few_rturn_min90_fast; break;
  case no_turn_90_fast: return (int)no_turn_min90_fast; break;
  case few_rturn_90_fast: return (int)few_lturn_min90_fast; break;
  case med_rturn_90_fast: return (int)med_lturn_min90_fast; break;
    //+-45 fast:
  case med_lturn_min45_fast: return (int)med_rturn_45_fast; break;
  case few_lturn_min45_fast: return (int)few_rturn_45_fast; break;
  case no_turn_min45_fast: return (int)no_turn_45_fast; break;
  case few_rturn_min45_fast: return (int)few_lturn_45_fast; break;
  case med_rturn_min45_fast: return (int)med_lturn_45_fast; break;
  case med_lturn_45_fast: return (int)med_rturn_min45_fast; break;
  case few_lturn_45_fast: return (int)few_rturn_min45_fast; break;
  case no_turn_45_fast: return (int)no_turn_min45_fast; break;
  case few_rturn_45_fast: return (int)few_lturn_min45_fast; break;
  case med_rturn_45_fast: return (int)med_lturn_min45_fast; break;
    //0+180, no turn:
  default: return -1;
  }
}

void GT2004ParametersSet::mirrorThis(int index)
{
  int mirror=getIndexOfMirror(index);
  if (mirror>=0)
  {
    GT2004Parameters* src=getParameters(index);
    GT2004Parameters* dest=getParameters(mirror);
    *dest = *src;
    dest->index=mirror;
    dest->requestedMotion.translation.y = -dest->requestedMotion.translation.y;
    dest->requestedMotion.rotation = -dest->requestedMotion.rotation;
    dest->correctedMotion.translation.y = -dest->correctedMotion.translation.y;
    dest->correctedMotion.rotation = -dest->correctedMotion.rotation;
  }
}

double GT2004ParametersSet::getSpeed(const Pose2D& request)
{ return sqrt(sqr(request.translation.abs()/300)+sqr(request.rotation/2.7)); }

double GT2004ParametersSet::getRatio(const Pose2D& request)
{ return atan2(request.rotation/2.7,request.translation.abs()/300)*2/pi; }

double GT2004ParametersSet::getDirection(const Pose2D& request)
{ return atan2(request.translation.y,request.translation.x); }

void GT2004ParametersSet::setSpeed(Pose2D& request, double speed)
{
  double ratio=getRatio(request);
  double direct=getDirection(request);
  request.rotation=2.7*sin(pi/2*ratio)*speed;
  request.translation.x=300*cos(pi/2*ratio)*cos(direct)*speed;
  request.translation.y=300*cos(pi/2*ratio)*sin(direct)*speed;
  if (fabs(request.rotation)<0.00001) {request.rotation=0;}
  if (fabs(request.translation.x)<0.001) {request.translation.x=0;}
  if (fabs(request.translation.y)<0.001) {request.translation.y=0;}
}

void GT2004ParametersSet::setRatio(Pose2D& request, double ratio)
{
  double speed=getSpeed(request);
  double direct=getDirection(request);
  request.rotation=2.7*sin(pi/2*ratio)*speed;
  request.translation.x=300*cos(pi/2*ratio)*cos(direct)*speed;
  request.translation.y=300*cos(pi/2*ratio)*sin(direct)*speed;
  if (fabs(request.rotation)<0.00001) {request.rotation=0;}
  if (fabs(request.translation.x)<0.001) {request.translation.x=0;}
  if (fabs(request.translation.y)<0.001) {request.translation.y=0;}
}

void GT2004ParametersSet::setDirection(Pose2D& request, double direct)
{
  double v=request.translation.abs();
  request.translation.x=cos(direct)*v;
  request.translation.y=sin(direct)*v;
  if (fabs(request.rotation)<0.00001) {request.rotation=0;}
  if (fabs(request.translation.x)<0.001) {request.translation.x=0;}
  if (fabs(request.translation.y)<0.001) {request.translation.y=0;}
}


bool GT2004ParametersSet::load(char* filename)
{
  char name[128];
  if (filename==0)
  {
    if (getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210)
    {
      sprintf(name,"udset210.dat");
    }
    else
    {
      sprintf(name,"udset7.dat");
    }
  }
  else
  {
    strcpy(name,filename);
  }
  InBinaryFile file(name);
  if (file.exists())
  {
    file >> *this;
    mergedParameters=&rotationOnly[3];
    return true;
  }
  return false;
}

void GT2004ParametersSet::save(char* filename)
{
  char name[128];
  if (filename==0)
  {
    if (getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210)
    {
      sprintf(name,"udset210.dat");
    }
    else
    {
      sprintf(name,"udset7.dat");
    }
  }
  else
  {
    strcpy(name,filename);
  }
  OutBinaryFile file(name);
  file << *this;
}

In& operator>>(In& stream,GT2004ParametersSet& walkingParametersSet)
{
  for (int i=0;i<=6;i++)
  {
    stream >> walkingParametersSet.rotationOnly[i];
  }
  for (int speed=0;speed<=2;speed++)
  {
    for (int rat=0;rat<=4;rat++)
    {
      for (int dir=0;dir<=7;dir++)
      {
        stream >> walkingParametersSet.withWalk[speed][rat][dir];
      }
    }
  }
  return stream;
}

Out& operator<<(Out& stream, const GT2004ParametersSet& walkingParametersSet)
{
  for (int i=0;i<=6;i++)
  {
    stream << walkingParametersSet.rotationOnly[i];
  }
  for (int speed=0;speed<=2;speed++)
  {
    for (int rat=0;rat<=4;rat++)
    {
      for (int dir=0;dir<=7;dir++)
      {
        stream << walkingParametersSet.withWalk[speed][rat][dir];
      }
    }
  }
  return stream;
}

/*
* Change log :
* 
* $Log: GT2004ParameterSet.cpp,v $
* Revision 1.1  2004/07/10 00:18:30  spranger
* renamed (readded) for coderelease
*
* Revision 1.4  2004/06/30 22:27:21  roefer
* Initialize some variables in constructor
*
* Revision 1.3  2004/05/29 18:20:14  dueffert
* cleanup
*
* Revision 1.2  2004/05/24 19:32:36  dueffert
* update to current version
*
* Revision 1.21  2004/05/24 19:30:51  dueffert
* bugs fixed
*
* Revision 1.20  2004/05/24 13:03:38  dueffert
* parameters fixed; complete lturn->rturn mirroring
*
* Revision 1.19  2004/05/20 17:19:52  dueffert
* cool parameters used now; request change methods moved to cpp
*
* Revision 1.18  2004/05/19 08:07:33  dueffert
* walk/turn-ratios tweaked
*
* Revision 1.17  2004/05/11 16:32:05  dueffert
* cool backward walking by Thomas works now in UD
*
* Revision 1.16  2004/05/03 18:52:38  dueffert
* comments corrected; max speed / rest distinction added
*
* Revision 1.15  2004/04/21 05:56:14  dueffert
* (temporary?) using BB for noturn_fastforward
*
* Revision 1.14  2004/03/29 15:25:25  dueffert
* maxStepSize hack for fixed parameters (from InvKin) for UD added
*
* Revision 1.13  2004/03/26 09:21:14  dueffert
* support for one UDParameters for everything added
*
* Revision 1.12  2004/03/17 11:56:41  dueffert
* initialization and naming corrected
*
* Revision 1.11  2004/03/09 08:55:44  dueffert
* constructor simplified
*
* Revision 1.10  2004/03/03 08:32:29  dueffert
* design specific load and save; reportRealMorion improved
*
* Revision 1.9  2004/03/01 14:55:07  dueffert
* reportRealMotion improved; now I use result of omni optimization as initialization
*
* Revision 1.8  2004/02/29 17:28:22  dueffert
* UDParameters have char* names now
*
* Revision 1.7  2004/02/29 13:38:47  dueffert
* symmetries in UDParametersSet handled
*
* Revision 1.6  2004/02/23 16:44:32  dueffert
* index names, load and save added
*
* Revision 1.5  2004/02/18 14:50:38  dueffert
* UDParameters improved
*
* Revision 1.4  2003/12/11 22:52:47  loetzsch
* fixed doxygen bugs
*
* Revision 1.3  2003/12/09 14:18:24  dueffert
* numerous improvements
*
* Revision 1.2  2003/12/04 16:42:30  dueffert
* now using cool new streaming operators
*
* Revision 1.1  2003/12/02 18:07:14  dueffert
* first working not yet calibrated version of UDWalkingEngine added
*
*
*/

---