Modules/WalkingEngine/GT2004WalkingEngine.cpp

Go to the documentation of this file.

/**
* @file GT2004WalkingEngine.cpp
* 
* Implementation of class GT2004WalkingEngine
*
* @author Uwe Düffert
*/

#include "GT2004WalkingEngine.h"
#include "Tools/Player.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "InvKinWalkingParameterSets.h"
#include "Tools/Debugging/Debugging.h"

GT2004WalkingEngine::GT2004WalkingEngine(const WalkingEngineInterfaces& interfaces):
WalkingEngine(interfaces),currentRequest(0,0,0), //currentStep(0),
currentStepPercentage(0),
paramInterpolCount(0),paramInterpolLength(0),
useFixedParameters(false),recalcEngineParameters(true),
lastParametersFromPackageTimeStamp(0)
{
  init();
}

GT2004WalkingEngine::~GT2004WalkingEngine()
{
}

void GT2004WalkingEngine::init()
{
  currentParameters=paramSet.mergedParameters;
  calculateParams();
}

void GT2004WalkingEngine::calculateParams()
{
  bodyTilt = asin((currentParameters->hindHeight - currentParameters->foreHeight)/lengthBetweenLegs);
  
  int i;
  for(i=0;i<2;i++) 
  {
    groundTime[i] = (int)(currentParameters->groundPhase[i] * currentParameters->stepLen);
    //liftTime of 2 looks better than immediate lifting for rectangle
    liftTime[i] = 2;
    loweringTime[i] = 2;

liftTime[i] = (int)(currentParameters->liftPhase[i] * currentParameters->stepLen); //temp for BB support
loweringTime[i] = (int)(currentParameters->loweringPhase[i] * currentParameters->stepLen); //temp for BB support
    
    airTime[i]=currentParameters->stepLen-groundTime[i]-liftTime[i]-loweringTime[i];
    stepLift[i]=groundTime[i];
    stepAir[i]=stepLift[i]+liftTime[i];
    stepLower[i]=stepAir[i]+airTime[i];
  }
  
  for (i=0;i<4;i++) {
    legPhaseIndex[i]=(int)(((double)currentParameters->stepLen)*currentParameters->legPhase[i]);
  }
  
  //2do: richtig rechnen oder aus Parametersatz oder Finger von lassen:
  neckHeight=120;
}

void GT2004WalkingEngine::setNextParameters(int steps)
{
  parametersToChange=paramSet.getParameters(nextParameters.index);
  lastParameters = *parametersToChange;
  initParametersInterpolation(steps);
}

void GT2004WalkingEngine::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 > 3.2)
    currentRequest.translation.x += 3.2;
  else if (request.translation.x - currentRequest.translation.x < -3.2)
    currentRequest.translation.x -= 3.2;
  else
    currentRequest.translation.x = request.translation.x;
  
  if (request.translation.y - currentRequest.translation.y > 3.2) 
    currentRequest.translation.y += 3.2;
  else if (request.translation.y - currentRequest.translation.y < -3.2)
    currentRequest.translation.y -= 3.2;
  else
    currentRequest.translation.y = request.translation.y;
  
  if (request.rotation - currentRequest.rotation > 0.032)
    currentRequest.rotation += 0.032;
  else if (request.rotation - currentRequest.rotation < -0.032)
    currentRequest.rotation -= 0.032;
  else
    currentRequest.rotation = request.rotation;
}

void GT2004WalkingEngine::calculateLegSpeeds()
{
  if (recalcEngineParameters)
  {
    if (!useFixedParameters)
    {
      paramSet.calculateMergedParameterSet(currentRequest);
      currentParameters=paramSet.mergedParameters;
      recalcEngineParameters=true;
    }
    calculateParams();
    recalcEngineParameters=false;
  }
  
  double stepSizeX;
  double stepSizeY;
  double stepSizeR;
  
  //it is not correct to "estimate" stepSizeR here by calculating the turn circle,
  //because stepSizeR is different for x- and y- component (see below),
  //therefore we only calculate x- and y- differences between foot stand positions,
  //average of foreWidth and hindWidth is used for that:
  double standWidth=(currentParameters->foreWidth+currentParameters->hindWidth)/2;
  double standLength=(currentParameters->foreCenterX+lengthBetweenLegs-currentParameters->hindCenterX)/2;

  if (useFixedParameters)
  {
    stepSizeX = currentRequest.translation.x * currentParameters->stepLen *motionCycleTime /4;
    stepSizeY = currentRequest.translation.y * currentParameters->stepLen *motionCycleTime /4;
    stepSizeR = currentRequest.rotation * currentParameters->stepLen *motionCycleTime /4;
    odometry = currentRequest;
  }
  else
  {
    stepSizeX = currentParameters->correctedMotion.translation.x * currentParameters->stepLen *motionCycleTime /4;
    stepSizeY = currentParameters->correctedMotion.translation.y * currentParameters->stepLen *motionCycleTime /4;
    stepSizeR = currentParameters->correctedMotion.rotation * currentParameters->stepLen *motionCycleTime /4;
    odometry = currentParameters->requestedMotion;
  }
  //for some reasons (eg walking on legs instead of feet) this is closer to reality:
  stepSizeR*=0.75;
  
  //linear movement
  legSpeed[Kinematics::fl].x=legSpeed[Kinematics::fr].x=legSpeed[Kinematics::hl].x=legSpeed[Kinematics::hr].x=
    stepSizeX;
  legSpeed[Kinematics::fl].y=legSpeed[Kinematics::fr].y=legSpeed[Kinematics::hl].y=legSpeed[Kinematics::hr].y=
    stepSizeY;
  
  //turn movement
  legSpeed[Kinematics::fl].x-=stepSizeR*standWidth;
  legSpeed[Kinematics::fr].x+=stepSizeR*standWidth;
  
  legSpeed[Kinematics::hl].x-=stepSizeR*standWidth;
  legSpeed[Kinematics::hr].x+=stepSizeR*standWidth;
  
  legSpeed[Kinematics::fl].y+=stepSizeR*standLength;
  legSpeed[Kinematics::fr].y+=stepSizeR*standLength;
  
  legSpeed[Kinematics::hl].y-=stepSizeR*standLength;
  legSpeed[Kinematics::hr].y-=stepSizeR*standLength;
  
  if (useFixedParameters)
  {
    //calculate foot point that is farest away from home position:
    double testX=fabs(stepSizeX)+fabs(stepSizeR*standWidth);
    double testY=fabs(stepSizeY)+fabs(stepSizeR*standLength);
    //calculate percentage of allowed (according to maxStepSize-Ellipse) distance from home position:
    double maxX=currentParameters->correctedMotion.translation.x<=0?38:currentParameters->correctedMotion.translation.x;
    double maxY=currentParameters->correctedMotion.translation.y<=0?42:currentParameters->correctedMotion.translation.y;
    double x=testX/maxX;
    double y=testY/maxY;
    //^4 instead of ^2 gives better results:
    //double factor = sqrt(x*x+y*y);
    double factor = sqrt(sqrt(x*x*x*x+y*y*y*y));
    if (factor>1)
    {
      legSpeed[Kinematics::fl] /= factor;
      legSpeed[Kinematics::fr] /= factor;
      legSpeed[Kinematics::hl] /= factor;
      legSpeed[Kinematics::hr] /= factor;
      odometry.translation /= factor;
      odometry.rotation /= factor;
    }
  }
  
  //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) * 100;
}

void GT2004WalkingEngine::calculateRelativeFootPosition(int step, int leg, double &rx, double &ry, double &rz)
{
  //calculate relative position in rectangle
  //rx = -1..1, rz = 0..1
  switch(currentParameters->footMode)
  {
  case GT2004Parameters::rectangle:
    if (step<stepLift[FORELEG(leg)?0:1]) {
      //foot is on ground
      rx=2.0*step/groundTime[FORELEG(leg)?0:1]-1.0;
      rz=0;
    } else if (step<stepAir[FORELEG(leg)?0:1]) {
      //raising foot
      rx=1.0;
      if (liftTime[FORELEG(leg)?0:1] == 0)
        rz=0;
      else
        rz=((double)(step-stepLift[FORELEG(leg)?0:1]))/liftTime[FORELEG(leg)?0:1];
    } else if (step<stepLower[FORELEG(leg)?0:1]) {
      //foot in air;
      if (airTime[FORELEG(leg)?0:1] == 0)
        rx=0;
      else
        rx=-2.0*(step-stepAir[FORELEG(leg)?0:1])/airTime[FORELEG(leg)?0:1]+1.0;
      rz=1.0;
    } else {
      //lowering foot
      rx=-1.0;
      if (loweringTime[FORELEG(leg)?0:1] == 0)
        rz=0;
      else
        rz=1.0-((double)(step-stepLower[FORELEG(leg)?0:1]))/loweringTime[FORELEG(leg)?0:1];
    }
    ry = 0;
    break;
  case GT2004Parameters::halfCircle:
    if (step<stepLift[FORELEG(leg)?0:1]) {
      //foot is on ground
      rx=2.0*step/groundTime[FORELEG(leg)?0:1]-1.0;
      rz=0;
/*
    } else {
      //foot in air;
      if (airTime[FORELEG(leg)?0:1] == 0) {
        rx=0;
        rz=1.0;
      } else {
        rx=cos(pi*(step-stepAir[FORELEG(leg)?0:1])/(liftTime[FORELEG(leg)?0:1]+airTime[FORELEG(leg)?0:1]+loweringTime[FORELEG(leg)?0:1]));
        rz=sin(pi*(step-stepAir[FORELEG(leg)?0:1])/(liftTime[FORELEG(leg)?0:1]+airTime[FORELEG(leg)?0:1]+loweringTime[FORELEG(leg)?0:1]));
      }
    }
*/

//temp for BB support:
} else if (step<stepAir[FORELEG(leg)?0:1]) {
 rx=1.0;
 rz=0;
} else if (step<stepLower[FORELEG(leg)?0:1]) {
 //foot in air;
 if (airTime[FORELEG(leg)?0:1] == 0) {
  rx=0;
  rz=1.0;
 } else {
  rx=cos(pi*(step-stepAir[FORELEG(leg)?0:1])/airTime[FORELEG(leg)?0:1]);
  rz=sin(pi*(step-stepAir[FORELEG(leg)?0:1])/airTime[FORELEG(leg)?0:1]);
 }
} else {
 //lowering foot
 rx=-1.0;
 rz=0;
}

    ry = 0;
    break;


  case GT2004Parameters::circle:
    if (step<stepLift[FORELEG(leg)?0:1])
    {
      //halfcircle through ground
      rx=-cos(pi*step/stepLift[FORELEG(leg)?0:1]);
      rz=-sin(pi*step/stepLift[FORELEG(leg)?0:1]);
    }
    else
    {
      //halfcircle through air
      rx=cos(pi*(step-stepLift[FORELEG(leg)?0:1])/(currentParameters->stepLen-stepLift[FORELEG(leg)?0:1]));
      rz=sin(pi*(step-stepLift[FORELEG(leg)?0:1])/(currentParameters->stepLen-stepLift[FORELEG(leg)?0:1]));
    }
    ry = 0;
    break;
  case GT2004Parameters::optimized:
  case GT2004Parameters::rounded:
  /*
  if (step<stepLift[FORELEG(leg)?0:1])
  {
  //foot is on ground
  rx=2.0*step/groundTime[FORELEG(leg)?0:1]-1.0;
  rz=0;
  }
  else if (step<stepAir[FORELEG(leg)?0:1])
  {
  //raising foot
  if (currentParameters->liftTime[FORELEG(leg)?0:1] == 0)
  {
  rx=1.0;
  rz=0;
  }
  else
  {
  rx=1.0+0.4*cos((((double)(step-currentParameters->stepLift[FORELEG(leg)?0:1]))/currentParameters->liftTime[FORELEG(leg)?0:1]-0.5)*pi);
  rz=0.5+0.5*sin((((double)(step-currentParameters->stepLift[FORELEG(leg)?0:1]))/currentParameters->liftTime[FORELEG(leg)?0:1]-0.5)*pi);
  }
  }
  else if (step<currentParameters->stepLower[FORELEG(leg)?0:1])
  {
  //foot in air;
  if (currentParameters->airTime[FORELEG(leg)?0:1] == 0)
  rx=0;
  else
  rx=-2.0*(step-currentParameters->stepAir[FORELEG(leg)?0:1])/currentParameters->airTime[FORELEG(leg)?0:1]+1.0;
  rz=1.0;
  }
  else
  {
  //lowering foot
  if (currentParameters->loweringTime[FORELEG(leg)?0:1] == 0)
  {
  rx=-1.0;
  rz=0;
  }
  else
  {
  rx=-1.0-0.4*cos((((double)(step-currentParameters->stepLower[FORELEG(leg)?0:1]))/currentParameters->loweringTime[FORELEG(leg)?0:1]-0.5)*pi);
  rz=0.5-0.5*sin((((double)(step-currentParameters->stepLower[FORELEG(leg)?0:1]))/currentParameters->loweringTime[FORELEG(leg)?0:1]-0.5)*pi);
  }
  }
    */
    ry = 0;
    break;
  case GT2004Parameters::freeFormQuad:
  /*
  double d;
  if (step<currentParameters->stepLift[FORELEG(leg)?0:1]) {
  //foot is on ground
  d = ((double)step)/currentParameters->groundTime[FORELEG(leg)?0:1];
  rx = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][0] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][0] * (1.0 - d);
  ry = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][1] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][1] * (1.0 - d);
  rz = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][2] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][2] * (1.0 - d);
  } else if (step<currentParameters->stepAir[FORELEG(leg)?0:1]) {
  //raising foot
  d = ((double)(step-currentParameters->stepLift[FORELEG(leg)?0:1]))/currentParameters->liftTime[FORELEG(leg)?0:1];
  rx = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][0] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][0] * (1.0 - d);
  ry = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][1] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][1] * (1.0 - d);
  rz = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][2] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][2] * (1.0 - d);
  } else if (step<currentParameters->stepLower[FORELEG(leg)?0:1]) {
  //foot in air;
  d = ((double)(step-currentParameters->stepAir[FORELEG(leg)?0:1]))/currentParameters->airTime[FORELEG(leg)?0:1];
  rx = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][0] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][0] * (1.0 - d);
  ry = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][1] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][1] * (1.0 - d);
  rz = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][2] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][2] * (1.0 - d);
  } else {
  //lowering foot
  d = ((double)(step-currentParameters->stepLower[FORELEG(leg)?0:1]))/currentParameters->loweringTime[FORELEG(leg)?0:1];
  rx = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][0] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][0] * (1.0 - d);
  ry = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][1] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][1] * (1.0 - d);
  rz = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][2] * d +
  currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][2] * (1.0 - d);
  }
    */
    break;
  }
}

void GT2004WalkingEngine::calculateFootPositions()
{
  double rx,ry,rz;
  int step,leg;
  //double bodyX = 0,bodyY = 0;
  
  for (leg=0;leg<4;leg++)
  {
    //leg center position on ground
    if (FORELEG(leg)) {
      footPos[leg].x=currentParameters->foreCenterX;
      footPos[leg].y=(LEFTLEG(leg))?currentParameters->foreWidth:-currentParameters->foreWidth;
      footPos[leg].z=-currentParameters->foreHeight;
    } else {
      footPos[leg].x=currentParameters->hindCenterX;
      footPos[leg].y=(LEFTLEG(leg))?currentParameters->hindWidth:-currentParameters->hindWidth;
      footPos[leg].z=-currentParameters->hindHeight;
    }
    
    //step for this leg
    step=(int)(currentStepPercentage*currentParameters->stepLen + legPhaseIndex[leg]);
    if (step>currentParameters->stepLen) step-=currentParameters->stepLen;
    
    calculateRelativeFootPosition(step, leg, rx, ry, rz);
    
    //that might be useful: footOnGround[leg] = (rz == 0);

    
    //calculate absolute position (center position + relative position)
    if (FORELEG(leg))
    {
      //on ERS7 sometimes .x looks much better than .abs(), but on ERS210 .abs() is much better...
      //so lift feet in walk-direction or in +x-direction:
      footPos[leg].z+=currentParameters->foreFootLift*rz;
      footPos[leg].z+=currentParameters->foreFootTilt*rx*legSpeed[leg].abs();
    }
    else
    {
      footPos[leg].z+=currentParameters->hindFootLift*rz;
      footPos[leg].z+=currentParameters->hindFootTilt*rx*legSpeed[leg].abs();
    }
    footPos[leg].x-=rx*legSpeed[leg].x;
    footPos[leg].y-=rx*legSpeed[leg].y;
    if (LEFTLEG(leg))
      footPos[leg].y+=ry;
    else
      footPos[leg].y-=ry;
  }
  /*
  //calculate shifted body center (away from lifted legs)
  if (currentParameters->bodyShiftX != 0 || currentParameters->bodyShiftY != 0)
  {
    for (leg=0;leg<4;leg++)
    {
      step=currentStepPercentage*currentParameters->stepLen + currentParameters->legPhaseIndex[leg]-(int)(currentParameters->bodyShiftOffset*currentParameters->stepLen);
      if (step>currentParameters->stepLen) step-=currentParameters->stepLen;
      calculateRelativeFootPosition(step, leg, rx, ry, rz);
      
      bodyY+=rz*currentParameters->bodyShiftY*((LEFTLEG(leg))?-1.0:1.0);
      bodyX+=rz*currentParameters->bodyShiftX*((FORELEG(leg))?-1.0:1.0);
    }
    
    for (leg=0;leg<4;leg++)
    {
      footPos[leg].x-=bodyX;
      footPos[leg].y-=bodyY;
    }
  }
  */
}

bool GT2004WalkingEngine::executeParameterized(JointData& jointData,
                                           const WalkRequest& walkRequest,
                                           double positionInWalkingCycle)
{
  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);

  //somebody put new gt2004Parameters into packageCognitionMotion, so lets use it:
  if (walkParameterTimeStamp>lastParametersFromPackageTimeStamp)
  {
    if (paramInterpolCount<paramInterpolLength)
    {
      //ensure end of last interpolation
      *parametersToChange=nextParameters;
    }
    nextParameters = gt2004Parameters;
    lastParametersFromPackageTimeStamp = walkParameterTimeStamp;
    if (nextParameters.index<GT2004ParametersSet::numberOfParameters)
    {
      useFixedParameters=false;
      currentParameters=paramSet.mergedParameters;
      setNextParameters(32);
    }
    else
    {
      useFixedParameters=true;
      lastParameters=*currentParameters;
      currentParameters=&fixedParameters;
      parametersToChange=&fixedParameters;
      initParametersInterpolation(32);
    }
  }

  Pose2D request = walkRequest.walkParams;
  bool standingStill = false;
  
  // reset current request if entered from other motion module
  if (lastMotionType != MotionRequest::walk)
  {
    currentRequest = Pose2D(0,0,0);
    currentStepPercentage = 0;
    // currentStep = 0;
  }
  else
  {
    currentStepPercentage = positionInWalkingCycle;
  }
  
  // calculate current request and new speeds
  if ((request != currentRequest || lastMotionType != MotionRequest::walk) ||
    recalcEngineParameters)
  {
    //lastMotionType=stand and lastMotionType=walk(0,0,0) are the same, but we do handle that rare changing case specially
    
    //prevent stumbling on quick motion request changes:
    smoothMotionRequest(request, currentRequest);
    if (!useFixedParameters)
    {
      recalcEngineParameters=true;
    }
    calculateLegSpeeds();
  }
  
  // stand still if request is zero
  standingStill = (
    currentRequest.translation.abs() < 10 &&
    fabs(currentRequest.rotation) < 0.09 );
  
  nextParametersInterpolation(!standingStill);
  
  odometryData.conc(odometry);
  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;
  
  calculateData(jointData);
  
  return false;
  //return ((currentStep!=1)&&(currentStep!=currentParameters->stepLen/2));
}

void GT2004WalkingEngine::calculateData(JointData &j) 
{
  double j1,j2,j3;
  
  calculateFootPositions();
  
  //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;
  
  //legs
  
  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;
    }
  }
}

bool GT2004WalkingEngine::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
  case idGT2004Parameters:
    if (paramInterpolCount<paramInterpolLength)
    {
      //ensure end of last interpolation
      *parametersToChange=nextParameters;
    }
    message.bin >> nextParameters;
    if (nextParameters.index<GT2004ParametersSet::numberOfParameters)
    {
      useFixedParameters=false;
      currentParameters=paramSet.mergedParameters;
      setNextParameters(32);
    }
    else
    {
      useFixedParameters=true;
      lastParameters=*currentParameters;
      currentParameters=&fixedParameters;
      parametersToChange=&fixedParameters;
      initParametersInterpolation(32);
    }
    return true;
  case idInvKinWalkingParameters:
    {
      if (paramInterpolCount<paramInterpolLength)
      {
        //ensure end of last interpolation
        *parametersToChange=nextParameters;
      }
      InvKinWalkingParameters invkin;
      message.bin >> invkin;
      nextParameters=invkin;
      useFixedParameters=true;
      lastParameters=*currentParameters;
      currentParameters=&fixedParameters;
      parametersToChange=&fixedParameters;
      initParametersInterpolation(32);
      return true;
    }
  case idGT2004EvolutionRequest:
    {
      int mode;
      message.bin >> mode;
      if (mode==3) // load parametersSet
      {
        paramSet.load();
        useFixedParameters=false;
        currentParameters=paramSet.mergedParameters;
        recalcEngineParameters=true;
        OUTPUT(idText,text,"GT2004WalkEng: GT2004EvolutionRequest 'load parametersSet' executed");
      }
      return true;
    }
  }
  return false;
}

void GT2004WalkingEngine::initParametersInterpolation(int changeSteps)
{
  paramInterpolCount=0;
  paramInterpolLength=changeSteps;
  if (changeSteps==0)
  {
    *parametersToChange=nextParameters;
  }
  recalcEngineParameters=true;
}

void GT2004WalkingEngine::nextParametersInterpolation(bool walk)
{
  if (paramInterpolCount<paramInterpolLength)
  {
    paramInterpolCount++;
    parametersToChange->interpolate(lastParameters,nextParameters,(double)(paramInterpolLength-paramInterpolCount)/paramInterpolLength);
    paramSet.mirrorThis(parametersToChange->index);
    recalcEngineParameters=true;
  }
  //currentParameters->stepLen may have changed due to interpolation, that does
  //not influence currentStepPercentage but currentStep has to be recalculated:
  if ((walk)||(currentStepPercentage!=0))
  {
    currentStepPercentage += 1.0 / (double)currentParameters->stepLen;
  }
  if (currentStepPercentage >= 1.0) 
    currentStepPercentage -= 1.0;
}

/*
* Change log :
* 
* $Log: GT2004WalkingEngine.cpp,v $
* Revision 1.1  2004/07/10 00:18:31  spranger
* renamed (readded) for coderelease
*
* Revision 1.9  2004/06/16 14:40:45  spranger
* - usage of positionInWalkCycle integrated
* - removed currentStep and doublestep
*
* Revision 1.8  2004/06/07 18:47:50  spranger
* extended interface of executeParametrized by positionInWalkCycle
*
* Revision 1.7  2004/06/02 17:18:22  spranger
* MotionRequest cleanup
*
* Revision 1.6  2004/05/27 17:13:37  jhoffman
* - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
* - clipping included for setJoints
* - removed some microrad/rad-bugs
* - bodyPosture constructor and "=" operator fixed
*
* Revision 1.5  2004/05/27 16:39:52  dueffert
* stand more
*
* Revision 1.4  2004/05/26 17:21:47  dueffert
* better data types used
*
* Revision 1.3  2004/05/25 10:58:28  dueffert
* return positionInWalkCycle
*
* Revision 1.2  2004/05/24 19:32:36  dueffert
* update to current version
*
* Revision 1.25  2004/05/24 19:31:21  dueffert
* improved PIDs
*
* Revision 1.24  2004/05/24 12:50:14  dueffert
* PID higher than default
*
* Revision 1.23  2004/05/20 17:20:38  dueffert
* comments corrected
*
* Revision 1.22  2004/05/19 08:07:57  dueffert
* calculation corrected to match reality
*
* Revision 1.21  2004/05/03 18:52:27  dueffert
* comments corrected
*
* Revision 1.20  2004/04/21 14:07:43  dueffert
* handling of incomming/changing parameters improved; Motion can load parameterset itself on request now
*
* Revision 1.19  2004/04/21 05:56:14  dueffert
* (temporary?) using BB for noturn_fastforward
*
* Revision 1.18  2004/03/29 15:25:49  dueffert
* maxStepSize hack for fixed parameters (from InvKin) for UD added
*
* Revision 1.17  2004/03/26 09:21:34  dueffert
* support for one UDParameters for everything added
*
* Revision 1.16  2004/03/16 10:28:59  dueffert
* handling for fast incoming parameters added; cooler walk stand transition
*
* Revision 1.15  2004/03/12 17:36:34  dueffert
* own bug fixed
*
* Revision 1.14  2004/03/12 17:10:30  dueffert
* rectangle mode enabled
*
* Revision 1.13  2004/03/10 15:03:31  dueffert
* ers7/210 difference comment added
*
* Revision 1.12  2004/03/10 09:59:14  dueffert
* serious calculation bug fixed and small smoothing improvements
*
* Revision 1.11  2004/03/08 02:11:55  roefer
* Interfaces should be const
*
* Revision 1.10  2004/02/29 13:38:47  dueffert
* symmetries in UDParametersSet handled
*
* Revision 1.9  2004/02/27 16:40:52  dueffert
* UDEvolutionRequest introduced
*
* Revision 1.8  2004/02/18 14:51:37  dueffert
* engine can now use new parameters provided via packageCognitionMotion and calculates StepSizeR correct
*
* Revision 1.7  2004/01/03 16:36:13  roefer
* motionCycleTime 8 ms -> 0.008 s
*
* Revision 1.6  2003/12/09 14:18:24  dueffert
* numerous improvements
*
* Revision 1.5  2003/12/05 13:05:20  dueffert
* sign bugs fixed
*
* Revision 1.4  2003/12/04 16:45:13  dueffert
* several minor bugs fixed
*
* Revision 1.3  2003/12/04 00:14:11  roefer
* New files added to VC2003
*
* Revision 1.2  2003/12/03 16:12:37  dueffert
* a bunch of minor bugfixes
*
* Revision 1.1  2003/12/02 18:07:14  dueffert
* first working not yet calibrated version of UDWalkingEngine added
*
*
*
*/

---