Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/WalkingEngine/GT2004WalkingEngine.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2004WalkingEngine.cpp
    00003 *
    00004 * Implementation of class GT2004WalkingEngine
    00005 *
    00006 * @author Uwe Düffert
    00007 */
    00008
    00009 #include "GT2004WalkingEngine.h"
    00010 #include "Tools/Player.h"
    00011 #include "Tools/RobotConfiguration.h"
    00012 #include "Tools/Streams/InStreams.h"
    00013 #include "Tools/Actorics/RobotDimensions.h"
    00014 #include "InvKinWalkingParameterSets.h"
    00015 #include "Tools/Debugging/Debugging.h"
    00016
    00017 GT2004WalkingEngine::GT2004WalkingEngine(const WalkingEngineInterfaces& interfaces):
    00018 WalkingEngine(interfaces),currentRequest(0,0,0), //currentStep(0),
    00019 currentStepPercentage(0),
    00020 paramInterpolCount(0),paramInterpolLength(0),
    00021 useFixedParameters(false),recalcEngineParameters(true),
    00022 lastParametersFromPackageTimeStamp(0)
    00023 {
    00024   init();
    00025 }
    00026
    00027 GT2004WalkingEngine::~GT2004WalkingEngine()
    00028 {
    00029 }
    00030
    00031 void GT2004WalkingEngine::init()
    00032 {
    00033   currentParameters=paramSet.mergedParameters;
    00034   calculateParams();
    00035 }
    00036
    00037 void GT2004WalkingEngine::calculateParams()
    00038 {
    00039   bodyTilt = asin((currentParameters->hindHeight - currentParameters->foreHeight)/lengthBetweenLegs);
    00040
    00041   int i;
    00042   for(i=0;i<2;i++)
    00043   {
    00044     groundTime[i] = (int)(currentParameters->groundPhase[i] * currentParameters->stepLen);
    00045     //liftTime of 2 looks better than immediate lifting for rectangle
    00046     liftTime[i] = 2;
    00047     loweringTime[i] = 2;
    00048
    00049 liftTime[i] = (int)(currentParameters->liftPhase[i] * currentParameters->stepLen); //temp for BB support
    00050 loweringTime[i] = (int)(currentParameters->loweringPhase[i] * currentParameters->stepLen); //temp for BB support
    00051
    00052     airTime[i]=currentParameters->stepLen-groundTime[i]-liftTime[i]-loweringTime[i];
    00053     stepLift[i]=groundTime[i];
    00054     stepAir[i]=stepLift[i]+liftTime[i];
    00055     stepLower[i]=stepAir[i]+airTime[i];
    00056   }
    00057
    00058   for (i=0;i<4;i++) {
    00059     legPhaseIndex[i]=(int)(((double)currentParameters->stepLen)*currentParameters->legPhase[i]);
    00060   }
    00061
    00062   //2do: richtig rechnen oder aus Parametersatz oder Finger von lassen:
    00063   neckHeight=120;
    00064 }
    00065
    00066 void GT2004WalkingEngine::setNextParameters(int steps)
    00067 {
    00068   parametersToChange=paramSet.getParameters(nextParameters.index);
    00069   lastParameters = *parametersToChange;
    00070   initParametersInterpolation(steps);
    00071 }
    00072
    00073 void GT2004WalkingEngine::smoothMotionRequest(const Pose2D& request, Pose2D& currentRequest)
    00074 {
    00075   //this is done per frame, so maximum speed change is 3.2mm/s/frame = 400mm/s^2
    00076   //and 0.032rad/s/frame = 4rad/s^2:
    00077   if (request.translation.x - currentRequest.translation.x > 3.2)
    00078     currentRequest.translation.x += 3.2;
    00079   else if (request.translation.x - currentRequest.translation.x < -3.2)
    00080     currentRequest.translation.x -= 3.2;
    00081   else
    00082     currentRequest.translation.x = request.translation.x;
    00083
    00084   if (request.translation.y - currentRequest.translation.y > 3.2)
    00085     currentRequest.translation.y += 3.2;
    00086   else if (request.translation.y - currentRequest.translation.y < -3.2)
    00087     currentRequest.translation.y -= 3.2;
    00088   else
    00089     currentRequest.translation.y = request.translation.y;
    00090
    00091   if (request.rotation - currentRequest.rotation > 0.032)
    00092     currentRequest.rotation += 0.032;
    00093   else if (request.rotation - currentRequest.rotation < -0.032)
    00094     currentRequest.rotation -= 0.032;
    00095   else
    00096     currentRequest.rotation = request.rotation;
    00097 }
    00098
    00099 void GT2004WalkingEngine::calculateLegSpeeds()
    00100 {
    00101   if (recalcEngineParameters)
    00102   {
    00103     if (!useFixedParameters)
    00104     {
    00105       paramSet.calculateMergedParameterSet(currentRequest);
    00106       currentParameters=paramSet.mergedParameters;
    00107       recalcEngineParameters=true;
    00108     }
    00109     calculateParams();
    00110     recalcEngineParameters=false;
    00111   }
    00112
    00113   double stepSizeX;
    00114   double stepSizeY;
    00115   double stepSizeR;
    00116
    00117   //it is not correct to "estimate" stepSizeR here by calculating the turn circle,
    00118   //because stepSizeR is different for x- and y- component (see below),
    00119   //therefore we only calculate x- and y- differences between foot stand positions,
    00120   //average of foreWidth and hindWidth is used for that:
    00121   double standWidth=(currentParameters->foreWidth+currentParameters->hindWidth)/2;
    00122   double standLength=(currentParameters->foreCenterX+lengthBetweenLegs-currentParameters->hindCenterX)/2;
    00123
    00124   if (useFixedParameters)
    00125   {
    00126     stepSizeX = currentRequest.translation.x * currentParameters->stepLen *motionCycleTime /4;
    00127     stepSizeY = currentRequest.translation.y * currentParameters->stepLen *motionCycleTime /4;
    00128     stepSizeR = currentRequest.rotation * currentParameters->stepLen *motionCycleTime /4;
    00129     odometry = currentRequest;
    00130   }
    00131   else
    00132   {
    00133     stepSizeX = currentParameters->correctedMotion.translation.x * currentParameters->stepLen *motionCycleTime /4;
    00134     stepSizeY = currentParameters->correctedMotion.translation.y * currentParameters->stepLen *motionCycleTime /4;
    00135     stepSizeR = currentParameters->correctedMotion.rotation * currentParameters->stepLen *motionCycleTime /4;
    00136     odometry = currentParameters->requestedMotion;
    00137   }
    00138   //for some reasons (eg walking on legs instead of feet) this is closer to reality:
    00139   stepSizeR*=0.75;
    00140
    00141   //linear movement
    00142   legSpeed[Kinematics::fl].x=legSpeed[Kinematics::fr].x=legSpeed[Kinematics::hl].x=legSpeed[Kinematics::hr].x=
    00143     stepSizeX;
    00144   legSpeed[Kinematics::fl].y=legSpeed[Kinematics::fr].y=legSpeed[Kinematics::hl].y=legSpeed[Kinematics::hr].y=
    00145     stepSizeY;
    00146
    00147   //turn movement
    00148   legSpeed[Kinematics::fl].x-=stepSizeR*standWidth;
    00149   legSpeed[Kinematics::fr].x+=stepSizeR*standWidth;
    00150
    00151   legSpeed[Kinematics::hl].x-=stepSizeR*standWidth;
    00152   legSpeed[Kinematics::hr].x+=stepSizeR*standWidth;
    00153
    00154   legSpeed[Kinematics::fl].y+=stepSizeR*standLength;
    00155   legSpeed[Kinematics::fr].y+=stepSizeR*standLength;
    00156
    00157   legSpeed[Kinematics::hl].y-=stepSizeR*standLength;
    00158   legSpeed[Kinematics::hr].y-=stepSizeR*standLength;
    00159
    00160   if (useFixedParameters)
    00161   {
    00162     //calculate foot point that is farest away from home position:
    00163     double testX=fabs(stepSizeX)+fabs(stepSizeR*standWidth);
    00164     double testY=fabs(stepSizeY)+fabs(stepSizeR*standLength);
    00165     //calculate percentage of allowed (according to maxStepSize-Ellipse) distance from home position:
    00166     double maxX=currentParameters->correctedMotion.translation.x<=0?38:currentParameters->correctedMotion.translation.x;
    00167     double maxY=currentParameters->correctedMotion.translation.y<=0?42:currentParameters->correctedMotion.translation.y;
    00168     double x=testX/maxX;
    00169     double y=testY/maxY;
    00170     //^4 instead of ^2 gives better results:
    00171     //double factor = sqrt(x*x+y*y);
    00172     double factor = sqrt(sqrt(x*x*x*x+y*y*y*y));
    00173     if (factor>1)
    00174     {
    00175       legSpeed[Kinematics::fl] /= factor;
    00176       legSpeed[Kinematics::fr] /= factor;
    00177       legSpeed[Kinematics::hl] /= factor;
    00178       legSpeed[Kinematics::hr] /= factor;
    00179       odometry.translation /= factor;
    00180       odometry.rotation /= factor;
    00181     }
    00182   }
    00183
    00184   //convert odometry from mm/s to mm/frame
    00185   odometry.translation *= motionCycleTime;
    00186   //convert odometry from rad/s to rad/frame
    00187   odometry.rotation *= motionCycleTime;
    00188
    00189   //correction for rotation center = foot middle instead of neck:
    00190   odometry.translation.y += sin(odometry.rotation) * 100;
    00191 }
    00192
    00193 void GT2004WalkingEngine::calculateRelativeFootPosition(int step, int leg, double &rx, double &ry, double &rz)
    00194 {
    00195   //calculate relative position in rectangle
    00196   //rx = -1..1, rz = 0..1
    00197   switch(currentParameters->footMode)
    00198   {
    00199   case GT2004Parameters::rectangle:
    00200     if (step<stepLift[FORELEG(leg)?0:1]) {
    00201       //foot is on ground
    00202       rx=2.0*step/groundTime[FORELEG(leg)?0:1]-1.0;
    00203       rz=0;
    00204     } else if (step<stepAir[FORELEG(leg)?0:1]) {
    00205       //raising foot
    00206       rx=1.0;
    00207       if (liftTime[FORELEG(leg)?0:1] == 0)
    00208         rz=0;
    00209       else
    00210         rz=((double)(step-stepLift[FORELEG(leg)?0:1]))/liftTime[FORELEG(leg)?0:1];
    00211     } else if (step<stepLower[FORELEG(leg)?0:1]) {
    00212       //foot in air;
    00213       if (airTime[FORELEG(leg)?0:1] == 0)
    00214         rx=0;
    00215       else
    00216         rx=-2.0*(step-stepAir[FORELEG(leg)?0:1])/airTime[FORELEG(leg)?0:1]+1.0;
    00217       rz=1.0;
    00218     } else {
    00219       //lowering foot
    00220       rx=-1.0;
    00221       if (loweringTime[FORELEG(leg)?0:1] == 0)
    00222         rz=0;
    00223       else
    00224         rz=1.0-((double)(step-stepLower[FORELEG(leg)?0:1]))/loweringTime[FORELEG(leg)?0:1];
    00225     }
    00226     ry = 0;
    00227     break;
    00228   case GT2004Parameters::halfCircle:
    00229     if (step<stepLift[FORELEG(leg)?0:1]) {
    00230       //foot is on ground
    00231       rx=2.0*step/groundTime[FORELEG(leg)?0:1]-1.0;
    00232       rz=0;
    00233 /*
    00234     } else {
    00235       //foot in air;
    00236       if (airTime[FORELEG(leg)?0:1] == 0) {
    00237         rx=0;
    00238         rz=1.0;
    00239       } else {
    00240         rx=cos(pi*(step-stepAir[FORELEG(leg)?0:1])/(liftTime[FORELEG(leg)?0:1]+airTime[FORELEG(leg)?0:1]+loweringTime[FORELEG(leg)?0:1]));
    00241         rz=sin(pi*(step-stepAir[FORELEG(leg)?0:1])/(liftTime[FORELEG(leg)?0:1]+airTime[FORELEG(leg)?0:1]+loweringTime[FORELEG(leg)?0:1]));
    00242       }
    00243     }
    00244 */
    00245
    00246 //temp for BB support:
    00247 } else if (step<stepAir[FORELEG(leg)?0:1]) {
    00248  rx=1.0;
    00249  rz=0;
    00250 } else if (step<stepLower[FORELEG(leg)?0:1]) {
    00251  //foot in air;
    00252  if (airTime[FORELEG(leg)?0:1] == 0) {
    00253   rx=0;
    00254   rz=1.0;
    00255  } else {
    00256   rx=cos(pi*(step-stepAir[FORELEG(leg)?0:1])/airTime[FORELEG(leg)?0:1]);
    00257   rz=sin(pi*(step-stepAir[FORELEG(leg)?0:1])/airTime[FORELEG(leg)?0:1]);
    00258  }
    00259 } else {
    00260  //lowering foot
    00261  rx=-1.0;
    00262  rz=0;
    00263 }
    00264
    00265     ry = 0;
    00266     break;
    00267
    00268
    00269   case GT2004Parameters::circle:
    00270     if (step<stepLift[FORELEG(leg)?0:1])
    00271     {
    00272       //halfcircle through ground
    00273       rx=-cos(pi*step/stepLift[FORELEG(leg)?0:1]);
    00274       rz=-sin(pi*step/stepLift[FORELEG(leg)?0:1]);
    00275     }
    00276     else
    00277     {
    00278       //halfcircle through air
    00279       rx=cos(pi*(step-stepLift[FORELEG(leg)?0:1])/(currentParameters->stepLen-stepLift[FORELEG(leg)?0:1]));
    00280       rz=sin(pi*(step-stepLift[FORELEG(leg)?0:1])/(currentParameters->stepLen-stepLift[FORELEG(leg)?0:1]));
    00281     }
    00282     ry = 0;
    00283     break;
    00284   case GT2004Parameters::optimized:
    00285   case GT2004Parameters::rounded:
    00286   /*
    00287   if (step<stepLift[FORELEG(leg)?0:1])
    00288   {
    00289   //foot is on ground
    00290   rx=2.0*step/groundTime[FORELEG(leg)?0:1]-1.0;
    00291   rz=0;
    00292   }
    00293   else if (step<stepAir[FORELEG(leg)?0:1])
    00294   {
    00295   //raising foot
    00296   if (currentParameters->liftTime[FORELEG(leg)?0:1] == 0)
    00297   {
    00298   rx=1.0;
    00299   rz=0;
    00300   }
    00301   else
    00302   {
    00303   rx=1.0+0.4*cos((((double)(step-currentParameters->stepLift[FORELEG(leg)?0:1]))/currentParameters->liftTime[FORELEG(leg)?0:1]-0.5)*pi);
    00304   rz=0.5+0.5*sin((((double)(step-currentParameters->stepLift[FORELEG(leg)?0:1]))/currentParameters->liftTime[FORELEG(leg)?0:1]-0.5)*pi);
    00305   }
    00306   }
    00307   else if (step<currentParameters->stepLower[FORELEG(leg)?0:1])
    00308   {
    00309   //foot in air;
    00310   if (currentParameters->airTime[FORELEG(leg)?0:1] == 0)
    00311   rx=0;
    00312   else
    00313   rx=-2.0*(step-currentParameters->stepAir[FORELEG(leg)?0:1])/currentParameters->airTime[FORELEG(leg)?0:1]+1.0;
    00314   rz=1.0;
    00315   }
    00316   else
    00317   {
    00318   //lowering foot
    00319   if (currentParameters->loweringTime[FORELEG(leg)?0:1] == 0)
    00320   {
    00321   rx=-1.0;
    00322   rz=0;
    00323   }
    00324   else
    00325   {
    00326   rx=-1.0-0.4*cos((((double)(step-currentParameters->stepLower[FORELEG(leg)?0:1]))/currentParameters->loweringTime[FORELEG(leg)?0:1]-0.5)*pi);
    00327   rz=0.5-0.5*sin((((double)(step-currentParameters->stepLower[FORELEG(leg)?0:1]))/currentParameters->loweringTime[FORELEG(leg)?0:1]-0.5)*pi);
    00328   }
    00329   }
    00330     */
    00331     ry = 0;
    00332     break;
    00333   case GT2004Parameters::freeFormQuad:
    00334   /*
    00335   double d;
    00336   if (step<currentParameters->stepLift[FORELEG(leg)?0:1]) {
    00337   //foot is on ground
    00338   d = ((double)step)/currentParameters->groundTime[FORELEG(leg)?0:1];
    00339   rx = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][0] * d +
    00340   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][0] * (1.0 - d);
    00341   ry = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][1] * d +
    00342   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][1] * (1.0 - d);
    00343   rz = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][2] * d +
    00344   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][2] * (1.0 - d);
    00345   } else if (step<currentParameters->stepAir[FORELEG(leg)?0:1]) {
    00346   //raising foot
    00347   d = ((double)(step-currentParameters->stepLift[FORELEG(leg)?0:1]))/currentParameters->liftTime[FORELEG(leg)?0:1];
    00348   rx = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][0] * d +
    00349   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][0] * (1.0 - d);
    00350   ry = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][1] * d +
    00351   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][1] * (1.0 - d);
    00352   rz = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][2] * d +
    00353   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][1][2] * (1.0 - d);
    00354   } else if (step<currentParameters->stepLower[FORELEG(leg)?0:1]) {
    00355   //foot in air;
    00356   d = ((double)(step-currentParameters->stepAir[FORELEG(leg)?0:1]))/currentParameters->airTime[FORELEG(leg)?0:1];
    00357   rx = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][0] * d +
    00358   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][0] * (1.0 - d);
    00359   ry = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][1] * d +
    00360   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][1] * (1.0 - d);
    00361   rz = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][2] * d +
    00362   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][2][2] * (1.0 - d);
    00363   } else {
    00364   //lowering foot
    00365   d = ((double)(step-currentParameters->stepLower[FORELEG(leg)?0:1]))/currentParameters->loweringTime[FORELEG(leg)?0:1];
    00366   rx = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][0] * d +
    00367   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][0] * (1.0 - d);
    00368   ry = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][1] * d +
    00369   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][1] * (1.0 - d);
    00370   rz = currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][0][2] * d +
    00371   currentParameters->freeFormQuadPos[FORELEG(leg)?0:1][3][2] * (1.0 - d);
    00372   }
    00373     */
    00374     break;
    00375   }
    00376 }
    00377
    00378 void GT2004WalkingEngine::calculateFootPositions()
    00379 {
    00380   double rx,ry,rz;
    00381   int step,leg;
    00382   //double bodyX = 0,bodyY = 0;
    00383
    00384   for (leg=0;leg<4;leg++)
    00385   {
    00386     //leg center position on ground
    00387     if (FORELEG(leg)) {
    00388       footPos[leg].x=currentParameters->foreCenterX;
    00389       footPos[leg].y=(LEFTLEG(leg))?currentParameters->foreWidth:-currentParameters->foreWidth;
    00390       footPos[leg].z=-currentParameters->foreHeight;
    00391     } else {
    00392       footPos[leg].x=currentParameters->hindCenterX;
    00393       footPos[leg].y=(LEFTLEG(leg))?currentParameters->hindWidth:-currentParameters->hindWidth;
    00394       footPos[leg].z=-currentParameters->hindHeight;
    00395     }
    00396
    00397     //step for this leg
    00398     step=(int)(currentStepPercentage*currentParameters->stepLen + legPhaseIndex[leg]);
    00399     if (step>currentParameters->stepLen) step-=currentParameters->stepLen;
    00400
    00401     calculateRelativeFootPosition(step, leg, rx, ry, rz);
    00402
    00403     //that might be useful: footOnGround[leg] = (rz == 0);
    00404
    00405
    00406     //calculate absolute position (center position + relative position)
    00407     if (FORELEG(leg))
    00408     {
    00409       //on ERS7 sometimes .x looks much better than .abs(), but on ERS210 .abs() is much better...
    00410       //so lift feet in walk-direction or in +x-direction:
    00411       footPos[leg].z+=currentParameters->foreFootLift*rz;
    00412       footPos[leg].z+=currentParameters->foreFootTilt*rx*legSpeed[leg].abs();
    00413     }
    00414     else
    00415     {
    00416       footPos[leg].z+=currentParameters->hindFootLift*rz;
    00417       footPos[leg].z+=currentParameters->hindFootTilt*rx*legSpeed[leg].abs();
    00418     }
    00419     footPos[leg].x-=rx*legSpeed[leg].x;
    00420     footPos[leg].y-=rx*legSpeed[leg].y;
    00421     if (LEFTLEG(leg))
    00422       footPos[leg].y+=ry;
    00423     else
    00424       footPos[leg].y-=ry;
    00425   }
    00426   /*
    00427   //calculate shifted body center (away from lifted legs)
    00428   if (currentParameters->bodyShiftX != 0 || currentParameters->bodyShiftY != 0)
    00429   {
    00430     for (leg=0;leg<4;leg++)
    00431     {
    00432       step=currentStepPercentage*currentParameters->stepLen + currentParameters->legPhaseIndex[leg]-(int)(currentParameters->bodyShiftOffset*currentParameters->stepLen);
    00433       if (step>currentParameters->stepLen) step-=currentParameters->stepLen;
    00434       calculateRelativeFootPosition(step, leg, rx, ry, rz);
    00435
    00436       bodyY+=rz*currentParameters->bodyShiftY*((LEFTLEG(leg))?-1.0:1.0);
    00437       bodyX+=rz*currentParameters->bodyShiftX*((FORELEG(leg))?-1.0:1.0);
    00438     }
    00439
    00440     for (leg=0;leg<4;leg++)
    00441     {
    00442       footPos[leg].x-=bodyX;
    00443       footPos[leg].y-=bodyY;
    00444     }
    00445   }
    00446   */
    00447 }
    00448
    00449 bool GT2004WalkingEngine::executeParameterized(JointData& jointData,
    00450                                            const WalkRequest& walkRequest,
    00451                                            double positionInWalkingCycle)
    00452 {
    00453   pidData.setLegFJ1Values(38, 8, 1);
    00454   pidData.setLegHJ1Values(38, 8, 1);
    00455   pidData.setLegFJ2Values(30, 4, 1);
    00456   pidData.setLegHJ2Values(30, 4, 1);
    00457   pidData.setLegFJ3Values(40, 8, 1);
    00458   pidData.setLegHJ3Values(40, 8, 1);
    00459
    00460   //somebody put new gt2004Parameters into packageCognitionMotion, so lets use it:
    00461   if (walkParameterTimeStamp>lastParametersFromPackageTimeStamp)
    00462   {
    00463     if (paramInterpolCount<paramInterpolLength)
    00464     {
    00465       //ensure end of last interpolation
    00466       *parametersToChange=nextParameters;
    00467     }
    00468     nextParameters = gt2004Parameters;
    00469     lastParametersFromPackageTimeStamp = walkParameterTimeStamp;
    00470     if (nextParameters.index<GT2004ParametersSet::numberOfParameters)
    00471     {
    00472       useFixedParameters=false;
    00473       currentParameters=paramSet.mergedParameters;
    00474       setNextParameters(32);
    00475     }
    00476     else
    00477     {
    00478       useFixedParameters=true;
    00479       lastParameters=*currentParameters;
    00480       currentParameters=&fixedParameters;
    00481       parametersToChange=&fixedParameters;
    00482       initParametersInterpolation(32);
    00483     }
    00484   }
    00485
    00486   Pose2D request = walkRequest.walkParams;
    00487   bool standingStill = false;
    00488
    00489   // reset current request if entered from other motion module
    00490   if (lastMotionType != MotionRequest::walk)
    00491   {
    00492     currentRequest = Pose2D(0,0,0);
    00493     currentStepPercentage = 0;
    00494     // currentStep = 0;
    00495   }
    00496   else
    00497   {
    00498     currentStepPercentage = positionInWalkingCycle;
    00499   }
    00500
    00501   // calculate current request and new speeds
    00502   if ((request != currentRequest || lastMotionType != MotionRequest::walk) ||
    00503     recalcEngineParameters)
    00504   {
    00505     //lastMotionType=stand and lastMotionType=walk(0,0,0) are the same, but we do handle that rare changing case specially
    00506
    00507     //prevent stumbling on quick motion request changes:
    00508     smoothMotionRequest(request, currentRequest);
    00509     if (!useFixedParameters)
    00510     {
    00511       recalcEngineParameters=true;
    00512     }
    00513     calculateLegSpeeds();
    00514   }
    00515
    00516   // stand still if request is zero
    00517   standingStill = (
    00518     currentRequest.translation.abs() < 10 &&
    00519     fabs(currentRequest.rotation) < 0.09 );
    00520
    00521   nextParametersInterpolation(!standingStill);
    00522
    00523   odometryData.conc(odometry);
    00524   motionInfo.neckHeight = neckHeight;
    00525   motionInfo.motionIsStable = true;
    00526   motionInfo.bodyTilt = bodyTilt +
    00527     getRobotConfiguration().getRobotCalibration().bodyTiltOffset;
    00528   motionInfo.executedMotionRequest.motionType = MotionRequest::walk;
    00529   motionInfo.executedMotionRequest.walkRequest.walkType = walkRequest.walkType;
    00530   motionInfo.executedMotionRequest.walkRequest.walkParams = currentRequest;
    00531   motionInfo.positionInWalkCycle = currentStepPercentage;
    00532
    00533   calculateData(jointData);
    00534
    00535   return false;
    00536   //return ((currentStep!=1)&&(currentStep!=currentParameters->stepLen/2));
    00537 }
    00538
    00539 void GT2004WalkingEngine::calculateData(JointData &j)
    00540 {
    00541   double j1,j2,j3;
    00542
    00543   calculateFootPositions();
    00544
    00545   //head, mouth
    00546   j.data[JointData::neckTilt] = currentParameters->headTilt;
    00547   j.data[JointData::headPan] = currentParameters->headPan;
    00548   j.data[JointData::headTilt] = currentParameters->headRoll;
    00549   j.data[JointData::mouth] = currentParameters->mouth;
    00550
    00551   //ears, tail
    00552   j.data[JointData::earL] = j.data[JointData::earR] =
    00553     j.data[JointData::tailTilt] = j.data[JointData::tailPan] =
    00554     jointDataInvalidValue;
    00555
    00556   //legs
    00557
    00558   for (int leg=0;leg<4;leg++)
    00559   {
    00560     if (Kinematics::jointsFromLegPosition((Kinematics::LegIndex)leg,footPos[leg].x,footPos[leg].y,footPos[leg].z,j1,j2,j3,bodyTilt))
    00561     {
    00562       j.data[JointData::legFR1+leg*3] = toMicroRad(j1);
    00563       j.data[JointData::legFR2+leg*3] = toMicroRad(j2);
    00564       j.data[JointData::legFR3+leg*3] = toMicroRad(j3);
    00565     }
    00566     else
    00567     {
    00568       j.data[JointData::tailTilt] = -300000;
    00569     }
    00570   }
    00571 }
    00572
    00573 bool GT2004WalkingEngine::handleMessage(InMessage& message)
    00574 {
    00575   switch(message.getMessageID())
    00576   {
    00577   case idGT2004Parameters:
    00578     if (paramInterpolCount<paramInterpolLength)
    00579     {
    00580       //ensure end of last interpolation
    00581       *parametersToChange=nextParameters;
    00582     }
    00583     message.bin >> nextParameters;
    00584     if (nextParameters.index<GT2004ParametersSet::numberOfParameters)
    00585     {
    00586       useFixedParameters=false;
    00587       currentParameters=paramSet.mergedParameters;
    00588       setNextParameters(32);
    00589     }
    00590     else
    00591     {
    00592       useFixedParameters=true;
    00593       lastParameters=*currentParameters;
    00594       currentParameters=&fixedParameters;
    00595       parametersToChange=&fixedParameters;
    00596       initParametersInterpolation(32);
    00597     }
    00598     return true;
    00599   case idInvKinWalkingParameters:
    00600     {
    00601       if (paramInterpolCount<paramInterpolLength)
    00602       {
    00603         //ensure end of last interpolation
    00604         *parametersToChange=nextParameters;
    00605       }
    00606       InvKinWalkingParameters invkin;
    00607       message.bin >> invkin;
    00608       nextParameters=invkin;
    00609       useFixedParameters=true;
    00610       lastParameters=*currentParameters;
    00611       currentParameters=&fixedParameters;
    00612       parametersToChange=&fixedParameters;
    00613       initParametersInterpolation(32);
    00614       return true;
    00615     }
    00616   case idGT2004EvolutionRequest:
    00617     {
    00618       int mode;
    00619       message.bin >> mode;
    00620       if (mode==3) // load parametersSet
    00621       {
    00622         paramSet.load();
    00623         useFixedParameters=false;
    00624         currentParameters=paramSet.mergedParameters;
    00625         recalcEngineParameters=true;
    00626         OUTPUT(idText,text,"GT2004WalkEng: GT2004EvolutionRequest 'load parametersSet' executed");
    00627       }
    00628       return true;
    00629     }
    00630   }
    00631   return false;
    00632 }
    00633
    00634 void GT2004WalkingEngine::initParametersInterpolation(int changeSteps)
    00635 {
    00636   paramInterpolCount=0;
    00637   paramInterpolLength=changeSteps;
    00638   if (changeSteps==0)
    00639   {
    00640     *parametersToChange=nextParameters;
    00641   }
    00642   recalcEngineParameters=true;
    00643 }
    00644
    00645 void GT2004WalkingEngine::nextParametersInterpolation(bool walk)
    00646 {
    00647   if (paramInterpolCount<paramInterpolLength)
    00648   {
    00649     paramInterpolCount++;
    00650     parametersToChange->interpolate(lastParameters,nextParameters,(double)(paramInterpolLength-paramInterpolCount)/paramInterpolLength);
    00651     paramSet.mirrorThis(parametersToChange->index);
    00652     recalcEngineParameters=true;
    00653   }
    00654   //currentParameters->stepLen may have changed due to interpolation, that does
    00655   //not influence currentStepPercentage but currentStep has to be recalculated:
    00656   if ((walk)||(currentStepPercentage!=0))
    00657   {
    00658     currentStepPercentage += 1.0 / (double)currentParameters->stepLen;
    00659   }
    00660   if (currentStepPercentage >= 1.0)
    00661     currentStepPercentage -= 1.0;
    00662 }