Modules/WalkingEngine/InvKinWalkingEngine.cpp

Go to the documentation of this file.

/**
* @file InvKinWalkingEngine.cpp
* 
* Implementation of class InvKinWalkingEngine
*
* @author Max Risler
*/

#include "InvKinWalkingEngine.h"
#include "Tools/Player.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "Platform/SystemCall.h"
#include <string.h>

InvKinWalkingEngine::InvKinWalkingEngine(const WalkingEngineInterfaces& interfaces)
: WalkingEngine(interfaces),
requestedParameters(0),paramInterpolCount(0),
paramInterpolLength(0), positionInWalkCycle(0),
lastParametersFromPackageTimeStamp(0)
{
  for (int i=0;i<4;i++) {
    legSpeedX[i]=0;legSpeedY[i]=0;
    footOnGround[i]=true;
  }
  currentRequest = Pose2D(0,0,0);
}

InvKinWalkingEngine::~InvKinWalkingEngine()
{
}

void InvKinWalkingEngine::setParameters(InvKinWalkingParameters* p, int changeSteps)
{
  if (requestedParameters != p)
  {
    nextParameters = *p;
    if (requestedParameters == NULL)
    {
      positionInWalkCycle = (double)currentParameters.firstStep/(double) currentParameters.stepLen;
      initParametersInterpolation(0);
    }
    else
    {
      initParametersInterpolation(changeSteps);
    }
    requestedParameters = p;
  }
}

bool InvKinWalkingEngine::executeParameterized(JointData& jointData,
                                               const WalkRequest& walkRequest,
                                               double requestedPositionInWalkCycle)
{
  //somebody put new invKinWalkingParameters into packageCognitionMotion, so lets use it:
  //(cloned from UDWalkingEngine.cpp)
  if (walkParameterTimeStamp>lastParametersFromPackageTimeStamp)
  {
    nextParameters.copyValuesFrom(invKinWalkingParameters);
    lastParametersFromPackageTimeStamp = walkParameterTimeStamp;
    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);
    positionInWalkCycle = (double)currentParameters.firstStep/(double)currentParameters.stepLen;
  }
  else
  {
    positionInWalkCycle = requestedPositionInWalkCycle;
  }
  
  // calculate new speeds and current request
  if (request != currentRequest ||
    lastMotionType != MotionRequest::walk)
  {
    // prevent stumbling on quick motion request changes 
    smoothMotionRequest(request, currentRequest);
    calculateLegSpeeds();
  }
  
  // stand still if request is zero
  if (currentRequest.translation.x == 0 &&
    currentRequest.translation.y == 0 &&
    currentRequest.rotation == 0)
    standingStill = true;
  
  nextParametersInterpolation(!standingStill || !currentParameters.allFeetOnGround((int)positionInWalkCycle * currentParameters.stepLen));
  
  odometryData.conc(odometry);
  
  motionInfo.neckHeight = currentParameters.neckHeight;
  
  motionInfo.bodyTilt = currentParameters.bodyTilt + 
#ifndef _WIN32
    currentParameters.bodyTiltOffset + 
#endif
    getRobotConfiguration().getRobotCalibration().bodyTiltOffset;
  
  motionInfo.motionIsStable = true;
  motionInfo.executedMotionRequest.motionType = MotionRequest::walk;
  motionInfo.executedMotionRequest.walkRequest.walkType = walkRequest.walkType;
  motionInfo.executedMotionRequest.walkRequest.walkParams = currentRequest;
  motionInfo.positionInWalkCycle = positionInWalkCycle;
  
  calculateData(jointData);
  
  return (!currentParameters.leaveAnytime && !currentParameters.allFeetOnGround((int)positionInWalkCycle * currentParameters.stepLen));
}

void InvKinWalkingEngine::calculateLegSpeeds() 
{
  double stepSizeX;
  double stepSizeY;
  double stepSizeR;

  odometry = currentRequest;

  // convert to internal leg speeds by multiplicating
  // with odometry correction values 
  if (currentRequest.translation.x > 0)
    stepSizeX = currentRequest.translation.x * currentParameters.correctionValues.forward;
  else
    stepSizeX = currentRequest.translation.x * currentParameters.correctionValues.backward;
  stepSizeY = currentRequest.translation.y * currentParameters.correctionValues.sideward;
  stepSizeR = currentRequest.rotation * currentParameters.correctionValues.turning;

  limitToMaxSpeed(stepSizeX,stepSizeY,stepSizeR);

  // correction for incorrect rotation center
  odometry.translation.y -= odometry.rotation * currentParameters.correctionValues.rotationCenter;

  // convert odometry from mm/s to mm/tick
  odometry.translation *= motionCycleTime;
  // convert odometry rad/s to rad/tick
  odometry.rotation *= motionCycleTime;

   // counterrotation to prevent unwanted rotation while walking sideways
  stepSizeR += stepSizeY * currentParameters.counterRotation;

  //linear movement
  legSpeedX[Kinematics::fl]=legSpeedX[Kinematics::fr]=legSpeedX[Kinematics::hl]=legSpeedX[Kinematics::hr]=
    stepSizeX;
  legSpeedY[Kinematics::fl]=legSpeedY[Kinematics::fr]=legSpeedY[Kinematics::hl]=legSpeedY[Kinematics::hr]=
    stepSizeY;

  //speed difference between fore and hind legs
  legSpeedX[Kinematics::fl]*=currentParameters.legSpeedFactorX;
  legSpeedX[Kinematics::fr]*=currentParameters.legSpeedFactorX;

  legSpeedY[Kinematics::fl]*=currentParameters.legSpeedFactorY;
  legSpeedY[Kinematics::fr]*=currentParameters.legSpeedFactorY;

  //turn movement
  legSpeedX[Kinematics::fl]-=stepSizeR*currentParameters.legSpeedFactorR;
  legSpeedX[Kinematics::fr]+=stepSizeR*currentParameters.legSpeedFactorR;

  legSpeedX[Kinematics::hl]-=stepSizeR;
  legSpeedX[Kinematics::hr]+=stepSizeR;
  
  legSpeedY[Kinematics::fl]+=stepSizeR*currentParameters.legSpeedFactorR;
  legSpeedY[Kinematics::fr]+=stepSizeR*currentParameters.legSpeedFactorR;

  legSpeedY[Kinematics::hr]-=stepSizeR;
  legSpeedY[Kinematics::hl]-=stepSizeR;
}

void InvKinWalkingEngine::smoothMotionRequest(const Pose2D& request, Pose2D& currentRequest)
{
  if (request.translation.x - currentRequest.translation.x > currentParameters.maxSpeedXChange) 
    currentRequest.translation.x += currentParameters.maxSpeedXChange;
  else if (request.translation.x - currentRequest.translation.x < -currentParameters.maxSpeedXChange)
    currentRequest.translation.x -= currentParameters.maxSpeedXChange;
  else
    currentRequest.translation.x = request.translation.x;

  if (request.translation.y - currentRequest.translation.y > currentParameters.maxSpeedYChange) 
    currentRequest.translation.y += currentParameters.maxSpeedYChange;
  else if (request.translation.y - currentRequest.translation.y < -currentParameters.maxSpeedYChange)
    currentRequest.translation.y -= currentParameters.maxSpeedYChange;
  else
    currentRequest.translation.y = request.translation.y;

  if (request.getAngle() - currentRequest.getAngle() > currentParameters.maxRotationChange)
    currentRequest.rotation += currentParameters.maxRotationChange;
  else if (request.getAngle() - currentRequest.getAngle() < -currentParameters.maxRotationChange)
    currentRequest.rotation -= currentParameters.maxRotationChange;
  else
    currentRequest.rotation = request.rotation;
}

void InvKinWalkingEngine::limitToMaxSpeed(double& stepSizeX, double& stepSizeY, double& stepSizeR)
{
  if (stepSizeR > currentParameters.maxStepSizeR) {
    stepSizeR = currentParameters.maxStepSizeR;
    odometry.rotation = currentParameters.maxStepSizeR / currentParameters.correctionValues.turning;
  } else if (stepSizeR < -currentParameters.maxStepSizeR) {
    stepSizeR = -currentParameters.maxStepSizeR;
    odometry.rotation = -currentParameters.maxStepSizeR / currentParameters.correctionValues.turning;
  }

  // Hier wird der Vektor (stepSizeX,stepSizeY) so innerhalb der Ellipse aus (maxStepSizeX,maxStepSizeY)
  // geklippt, dass danach jeder der Vektoren für die einzelnen Beine (stepSizeX+-stepSizeR,stepSizeY+-stepSizeR)
  // noch innerhalb der Ellipse liegt. Die Richtung von (stepSizeX,stepSizeY) wird dabei nicht verändert.
  // Wer dies ändern will möge das mit mir absprechen.
  // Das geht evtl auch einfacher aber die Funktionalität soll erhalten bleiben. MR

  double testX = stepSizeX;
  double testY = stepSizeY;
  double r = fabs(stepSizeR);

  // generate point for greatest step size including rotation
  if (testX>0) testX += r; else testX -= r;
  if (testY>0) testY += r; else testY -= r;

  // test if maxStepSize exceeded = check if (testX,testY) is inside ellipse described by maxStepSizeX,maxStepSizeY
  if ((testX*testX/(currentParameters.maxStepSizeX*currentParameters.maxStepSizeX) +
       testY*testY/(currentParameters.maxStepSizeY*currentParameters.maxStepSizeY)) > 1.0)
  {
    // step size outside of ellipse described by maxStepSizeX,maxStepSizeY
    // calculate point on ellipse with direction of testX,testY
    if (testX != 0)
    {
      double m = testY / testX;
      stepSizeX = 
        ((testX>0)?1.0:-1.0) *
        currentParameters.maxStepSizeX*currentParameters.maxStepSizeY / 
        sqrt(currentParameters.maxStepSizeY*currentParameters.maxStepSizeY + 
                     currentParameters.maxStepSizeX*currentParameters.maxStepSizeX*m*m);
      stepSizeY = m*stepSizeX;
    } else {
      stepSizeX = 0;
      stepSizeY = ((testY>0)?1.0:-1.0) * currentParameters.maxStepSizeY;
    }
    // subtract rotation
    if (stepSizeX>0) stepSizeX -= r; else stepSizeX += r;
    if (stepSizeY>0) stepSizeY -= r; else stepSizeY += r;
    
    if (stepSizeX > 0)
      odometry.translation.x = stepSizeX / currentParameters.correctionValues.forward;
    else
      odometry.translation.x = stepSizeX / currentParameters.correctionValues.backward;
    odometry.translation.y = stepSizeY / currentParameters.correctionValues.sideward;
  }
}


void InvKinWalkingEngine::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,x[leg],y[leg],z[leg],j1,j2,j3,currentParameters.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;
    }
  }
}

double InvKinWalkingEngine::getLegPositionCurve(double& rz,double index)
{
  const double xtab[22]={1.0, 1.05, 1.1,  1.155, 1.19, 1.185, 1.165, 1.125, 1.07, 1.01, 0.94, 0.86, 0.77, 0.683, 0.59, 0.5,  0.4,  0.3,   0.2, 0.1, 0.0, 0.0};
  const double ztab[22]={0.0, 0.03, 0.095, 0.18, 0.27, 0.37,  0.47,  0.565, 0.65, 0.74, 0.8,  0.85, 0.9,  0.935, 0.96, 0.98, 0.99, 0.995, 1.0, 1.0, 1.0, 1.0};
  double ind=4*20*index;
  if (ind>20) ind=20;
  if (ind<0) ind=0;
  int left=(int)ind;
  int right=left+1;
  double weightRight=ind-left;
  double weightLeft=1.0-weightRight;
  rz=weightLeft*ztab[left]+weightRight*ztab[right];
  return weightLeft*xtab[left]+weightRight*xtab[right];
}

void InvKinWalkingEngine::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 InvKinWalkingParameters::rectangle:
    if (step<currentParameters.stepLift[FORELEG(leg)?0:1]) {
      //foot is on ground
      rx=2.0*step/currentParameters.groundTime[FORELEG(leg)?0:1]-1.0;
      rz=0;
    } else if (step<currentParameters.stepAir[FORELEG(leg)?0:1]) {
      //raising foot
      rx=1.0;
      if (currentParameters.liftTime[FORELEG(leg)?0:1] == 0)
        rz=0;
      else
        rz=((double)(step-currentParameters.stepLift[FORELEG(leg)?0:1]))/currentParameters.liftTime[FORELEG(leg)?0:1];
    } 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
      rx=-1.0;
      if (currentParameters.loweringTime[FORELEG(leg)?0:1] == 0)
        rz=0;
      else
        rz=1.0-((double)(step-currentParameters.stepLower[FORELEG(leg)?0:1]))/currentParameters.loweringTime[FORELEG(leg)?0:1];
    }
    ry = 0;
    break;
  case InvKinWalkingParameters::halfCircle:
    if (step<currentParameters.stepLift[FORELEG(leg)?0:1]) {
      //foot is on ground
      rx=2.0*step/currentParameters.groundTime[FORELEG(leg)?0:1]-1.0;
      rz=0;
    } else if (step<currentParameters.stepAir[FORELEG(leg)?0:1]) {
      rx=1.0;
      rz=0;
    } else if (step<currentParameters.stepLower[FORELEG(leg)?0:1]) {
      //foot in air;
      if (currentParameters.airTime[FORELEG(leg)?0:1] == 0) {
        rx=0;
        rz=1.0;
      } else {
        rx=cos(pi*(step-currentParameters.stepAir[FORELEG(leg)?0:1])/currentParameters.airTime[FORELEG(leg)?0:1]);
        rz=sin(pi*(step-currentParameters.stepAir[FORELEG(leg)?0:1])/currentParameters.airTime[FORELEG(leg)?0:1]);
      }
    } else {
      //lowering foot
      rx=-1.0;
      rz=0;
    }
    ry = 0;
    break;
  case InvKinWalkingParameters::circle:
    if (step<currentParameters.stepLift[FORELEG(leg)?0:1])
    {
      //halfcircle through ground
      rx=-cos(pi*step/currentParameters.stepLift[FORELEG(leg)?0:1]);
      rz=-sin(pi*step/currentParameters.stepLift[FORELEG(leg)?0:1]);
    }
    else
    {
      //halfcircle through air
      rx=cos(pi*(step-currentParameters.stepLift[FORELEG(leg)?0:1])/(currentParameters.stepLen-currentParameters.stepLift[FORELEG(leg)?0:1]));
      rz=sin(pi*(step-currentParameters.stepLift[FORELEG(leg)?0:1])/(currentParameters.stepLen-currentParameters.stepLift[FORELEG(leg)?0:1]));
    }
    ry = 0;
    break;
  case InvKinWalkingParameters::optimized:
    if (step<currentParameters.stepLift[FORELEG(leg)?0:1])
    {
      //foot is on ground
      rx=2.0*step/currentParameters.groundTime[FORELEG(leg)?0:1]-1.0;
      rz=0;
    }
    else
    {
      int x=step-currentParameters.stepLift[FORELEG(leg)?0:1];
      if (x < (currentParameters.stepLen-currentParameters.groundTime[FORELEG(leg)?0:1])/2)
      {
        rx=getLegPositionCurve(rz,((double)x)/currentParameters.stepLen);
      }
      else
      {
        rx=-getLegPositionCurve(rz,((double)(currentParameters.stepLen-currentParameters.groundTime[FORELEG(leg)?0:1])-x)/currentParameters.stepLen);
      }
    }
    ry = 0;
    break;
  case InvKinWalkingParameters::rounded:
    if (step<currentParameters.stepLift[FORELEG(leg)?0:1])
    {
      //foot is on ground
      rx=2.0*step/currentParameters.groundTime[FORELEG(leg)?0:1]-1.0;
      rz=0;
    }
    else if (step<currentParameters.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 InvKinWalkingParameters::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 InvKinWalkingEngine::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)) {
      x[leg]=currentParameters.foreCenterX;
      y[leg]=(LEFTLEG(leg))?currentParameters.foreWidth:-currentParameters.foreWidth;
      z[leg]=-currentParameters.foreHeight;
    } else {
      x[leg]=currentParameters.hindCenterX;
      y[leg]=(LEFTLEG(leg))?currentParameters.hindWidth:-currentParameters.hindWidth;
      z[leg]=-currentParameters.hindHeight;
    }

    //step for this leg
    step=(((int)(positionInWalkCycle * currentParameters.stepLen)) + currentParameters.legPhaseIndex[leg]);
    if (step>currentParameters.stepLen) step-=currentParameters.stepLen;

    calculateRelativeFootPosition(step, leg, rx, ry, rz);

    footOnGround[leg] = (rz == 0);

    //calculate absolute position (center position + relative position)
    if (FORELEG(leg))
    {
      z[leg]+=currentParameters.foreFootLift*rz;
      z[leg]+=currentParameters.foreFootTilt*rx*legSpeedX[leg];
    } else {
      z[leg]+=currentParameters.hindFootLift*rz;
      z[leg]+=currentParameters.hindFootTilt*rx*legSpeedX[leg];
    }
    x[leg]-=rx*legSpeedX[leg];
    y[leg]-=rx*legSpeedY[leg];
    if (LEFTLEG(leg))
      y[leg]+=ry;
    else
      y[leg]-=ry;
  }

  //calculate shifted body center (away from lifted legs)
  if (currentParameters.bodyShiftX != 0 || currentParameters.bodyShiftY != 0)
  {
    for (leg=0;leg<4;leg++)
    {
      step=((int)(positionInWalkCycle * 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++)
    {
      x[leg]-=bodyX;
      y[leg]-=bodyY;
    }
  }
}

bool InvKinWalkingEngine::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
    case idInvKinWalkingParameters:
      message.bin >> nextParameters;
      initParametersInterpolation(32);
      return true;
  }
  return false;
}

void InvKinWalkingEngine::initParametersInterpolation(int changeSteps)
{
  lastParameters=currentParameters;
  paramInterpolCount=0;
  paramInterpolLength=changeSteps;
  if (changeSteps==0)
  {
    currentParameters=nextParameters;
  }
}

void InvKinWalkingEngine::nextParametersInterpolation(bool walk)
{
  if (paramInterpolCount<paramInterpolLength)
  {
    paramInterpolCount++;
    currentParameters.interpolate(lastParameters,nextParameters,(double)(paramInterpolLength-paramInterpolCount)/paramInterpolLength);
    currentParameters.init();
    calculateLegSpeeds();
    positionInWalkCycle += ((walk)?1:0) * 1/(double) currentParameters.stepLen;
  }
  else if (walk)
  {
    positionInWalkCycle += ((walk)?1:0) * 1/(double) currentParameters.stepLen;
  }
  if (positionInWalkCycle>=1) positionInWalkCycle-=1;
}

---