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

# Modules/WalkingEngine/GT2004ParameterSet.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2004ParameterSet.cpp
    00003 *
    00004 * Implementation of class GT2004Parameters and GT2004ParametersSet
    00005 *
    00006 * @author Uwe Düffert
    00007 */
    00008
    00009 #include "GT2004ParameterSet.h"
    00010 #include "Modules/WalkingEngine/InvKinWalkingParameterSets.h"
    00011 #include "Tools/Streams/InStreams.h"
    00012 #include "Tools/Streams/OutStreams.h"
    00013
    00014 GT2004Parameters::GT2004Parameters(const InvKinWalkingParameters& invKinParam)
    00015 {
    00016   int i;
    00017
    00018   index=GT2004ParametersSet::numberOfParameters;
    00019   requestedMotion=Pose2D(0,0,0);
    00020   correctedMotion=Pose2D(0,invKinParam.maxStepSizeX,invKinParam.maxStepSizeY);
    00021
    00022   foreHeight =invKinParam.foreHeight;
    00023   foreWidth  =invKinParam.foreWidth;
    00024   foreCenterX=invKinParam.foreCenterX;
    00025
    00026   hindHeight =invKinParam.hindHeight;
    00027   hindWidth  =invKinParam.hindWidth;
    00028   hindCenterX=invKinParam.hindCenterX;
    00029
    00030   foreFootLift=invKinParam.foreFootLift;
    00031   hindFootLift=invKinParam.hindFootLift;
    00032
    00033   foreFootTilt=invKinParam.foreFootTilt;
    00034   hindFootTilt=invKinParam.hindFootTilt;
    00035
    00036   stepLen=invKinParam.stepLen;
    00037
    00038   switch(invKinParam.footMode)
    00039   {
    00040   case InvKinWalkingParameters::rectangle:
    00041   case InvKinWalkingParameters::freeFormQuad:
    00042     footMode=rectangle;
    00043     break;
    00044   case InvKinWalkingParameters::halfCircle:
    00045   case InvKinWalkingParameters::circle:
    00046   case InvKinWalkingParameters::rounded:
    00047   case InvKinWalkingParameters::optimized:
    00048   default:
    00049     footMode=halfCircle;
    00050     break;
    00051   }
    00052
    00053   for (i=0;i<2;i++)
    00054   {
    00055     groundPhase[i]=invKinParam.groundPhase[i];
    00056 liftPhase[i]=invKinParam.liftPhase[i];         //temp for BB support
    00057 loweringPhase[i]=invKinParam.loweringPhase[i]; //temp for BB support
    00058   }
    00059 //bodyShiftX= invKinParam.bodyShiftX; //temp for BB support
    00060 //bodyShiftY= invKinParam.bodyShiftY; //temp for BB support
    00061 //bodyShiftOffset= invKinParam.bodyShiftOffset; //temp for BB support
    00062   for (i=0;i<4;i++)
    00063   {
    00064     legPhase[i]=invKinParam.legPhase[i];
    00065   }
    00066
    00067   headTilt=invKinParam.headTilt;
    00068   headPan=invKinParam.headPan;
    00069   headRoll=invKinParam.headRoll;
    00070   mouth=invKinParam.mouth;
    00071
    00072   fitness=0;
    00073 }
    00074
    00075 bool GT2004Parameters::reportRealMotion(const Pose2D& real)
    00076 {
    00077   bool result=true;
    00078   correctedMotion.translation += (requestedMotion.translation-real.translation)/2;
    00079   correctedMotion.rotation += (requestedMotion.rotation-real.rotation)/2;
    00080   //allow no more than additional 23 percent on higher values:
    00081   if ((fabs(correctedMotion.rotation)>1.23*fabs(requestedMotion.rotation))&&(fabs(correctedMotion.rotation)>1))
    00082   {
    00083     correctedMotion.rotation=1.23*requestedMotion.rotation;
    00084     result=false;
    00085   }
    00086   if ((fabs(correctedMotion.translation.x)>1.23*fabs(requestedMotion.translation.x))&&(fabs(correctedMotion.translation.x)>150))
    00087   {
    00088     correctedMotion.translation.x=1.23*requestedMotion.translation.x;
    00089     result=false;
    00090   }
    00091   if ((fabs(correctedMotion.translation.y)>1.23*fabs(requestedMotion.translation.y))&&(fabs(correctedMotion.translation.y)>150))
    00092   {
    00093     correctedMotion.translation.y=1.23*requestedMotion.translation.y;
    00094     result=false;
    00095   }
    00096   return result;
    00097 }
    00098
    00099 In& operator>>(In& stream,GT2004Parameters& walkingParameters)
    00100 {
    00101   stream >> walkingParameters.index;
    00102   stream >> walkingParameters.requestedMotion;
    00103   stream >> walkingParameters.correctedMotion;
    00104
    00105   stream >> walkingParameters.foreHeight;
    00106   stream >> walkingParameters.foreWidth;
    00107   stream >> walkingParameters.foreCenterX;
    00108   stream >> walkingParameters.hindHeight;
    00109   stream >> walkingParameters.hindWidth;
    00110   stream >> walkingParameters.hindCenterX;
    00111   stream >> walkingParameters.foreFootLift;
    00112   stream >> walkingParameters.hindFootLift;
    00113   stream >> walkingParameters.foreFootTilt;
    00114   stream >> walkingParameters.hindFootTilt;
    00115   stream >> walkingParameters.stepLen;
    00116   int m;
    00117   stream >> m;
    00118   walkingParameters.footMode = (GT2004Parameters::FootMode)m;
    00119   stream >> walkingParameters.groundPhase[0];
    00120 stream >> walkingParameters.liftPhase[0];  //temp for BB support
    00121 stream >> walkingParameters.loweringPhase[0];  //temp for BB support
    00122   stream >> walkingParameters.groundPhase[1];
    00123 stream >> walkingParameters.liftPhase[1];  //temp for BB support
    00124 stream >> walkingParameters.loweringPhase[1];  //temp for BB support
    00125 //stream >> walkingParameters.bodyShiftX; //temp for BB support
    00126 //stream >> walkingParameters.bodyShiftY; //temp for BB support
    00127 //stream >> walkingParameters.bodyShiftOffset; //temp for BB support
    00128   stream >> walkingParameters.legPhase[0];
    00129   stream >> walkingParameters.legPhase[1];
    00130   stream >> walkingParameters.legPhase[2];
    00131   stream >> walkingParameters.legPhase[3];
    00132
    00133   return stream;
    00134 }
    00135
    00136 Out& operator<<(Out& stream, const GT2004Parameters& walkingParameters)
    00137 {
    00138   stream << walkingParameters.index;
    00139   stream << walkingParameters.requestedMotion;
    00140   stream << walkingParameters.correctedMotion;
    00141
    00142   stream << walkingParameters.foreHeight;
    00143   stream << walkingParameters.foreWidth;
    00144   stream << walkingParameters.foreCenterX;
    00145   stream << walkingParameters.hindHeight;
    00146   stream << walkingParameters.hindWidth;
    00147   stream << walkingParameters.hindCenterX;
    00148   stream << walkingParameters.foreFootLift;
    00149   stream << walkingParameters.hindFootLift;
    00150   stream << walkingParameters.foreFootTilt;
    00151   stream << walkingParameters.hindFootTilt;
    00152   stream << walkingParameters.stepLen;
    00153   stream << (int)walkingParameters.footMode;
    00154   stream << walkingParameters.groundPhase[0];
    00155 stream << walkingParameters.liftPhase[0];  //temp for BB support
    00156 stream << walkingParameters.loweringPhase[0];  //temp for BB support
    00157   stream << walkingParameters.groundPhase[1];
    00158 stream << walkingParameters.liftPhase[1];  //temp for BB support
    00159 stream << walkingParameters.loweringPhase[1];  //temp for BB support
    00160 //stream << walkingParameters.bodyShiftX; //temp for BB support
    00161 //stream << walkingParameters.bodyShiftY; //temp for BB support
    00162 //stream << walkingParameters.bodyShiftOffset; //temp for BB support
    00163   stream << walkingParameters.legPhase[0];
    00164   stream << walkingParameters.legPhase[1];
    00165   stream << walkingParameters.legPhase[2];
    00166   stream << walkingParameters.legPhase[3];
    00167
    00168   return stream;
    00169 }
    00170
    00171 GT2004ParametersSet::GT2004ParametersSet()
    00172 {
    00173   lowRatioLowSpeed=&withWalk[0][0][0];
    00174   lowRatioUpSpeed=&withWalk[0][0][0];
    00175   upRatioLowSpeed=&withWalk[0][0][0];
    00176   upRatioUpSpeed=&withWalk[0][0][0];
    00177   lowSpeed=lowRatioLowSpeed;
    00178   upSpeed=lowRatioUpSpeed;
    00179   mergedParameters=lowSpeed;
    00180
    00181   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);
    00182
    00183   //BB original from walking.cfg:
    00184   //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);
    00185   //0 instead of 0.98 to allow interpolation:
    00186   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);
    00187   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);
    00188   //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);
    00189   //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);
    00190   //negative tilts because GT2004 uses .abs()*tilt instead of .x*tilt:
    00191   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);
    00192   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);
    00193
    00194   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);
    00195   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);
    00196
    00197   for (int i=-3;i<=3;i++)
    00198   {
    00199     rotationOnly[3+i]=(abs(i)==3)?udTurn:msh7;//invEvo;
    00200     rotationOnly[3+i].index=3+i;
    00201     rotationOnly[3+i].requestedMotion=Pose2D(0.7*i,0,0);
    00202     rotationOnly[3+i].correctedMotion=rotationOnly[3+i].requestedMotion;
    00203   }
    00204   rotationOnly[0].requestedMotion=Pose2D(-4.25,0,0);
    00205   rotationOnly[0].correctedMotion=rotationOnly[0].requestedMotion;
    00206   rotationOnly[6].requestedMotion=Pose2D(4.25,0,0);
    00207   rotationOnly[6].correctedMotion=rotationOnly[6].requestedMotion;
    00208
    00209   static const double ratios[]={-0.3*pi_2,-0.1*pi_2,0,0.1*pi_2,0.3*pi_2};
    00210   for (int dir=-4;dir<4;dir++)
    00211   {
    00212     for (int rat=-2;rat<=2;rat++)
    00213     {
    00214       withWalk[2][rat+2][dir+4]=msh7;//invEvo;
    00215       withWalk[2][rat+2][dir+4].index=7+5*8*2+8*(rat+2)+(dir+4);
    00216       withWalk[2][rat+2][dir+4].requestedMotion=Pose2D(
    00217         sin(ratios[rat+2])*0.9*2.7,
    00218         cos(ratios[rat+2])*0.9*300*cos(dir*pi_4),
    00219         cos(ratios[rat+2])*0.9*300*sin(dir*pi_4));
    00220       withWalk[2][rat+2][dir+4].correctedMotion=withWalk[2][rat+2][dir+4].requestedMotion;
    00221
    00222       withWalk[1][rat+2][dir+4]=msh7; //invEvo; //invQU;
    00223       withWalk[1][rat+2][dir+4].index=7+5*8*1+8*(rat+2)+(dir+4);
    00224       withWalk[1][rat+2][dir+4].requestedMotion=Pose2D(
    00225         sin(ratios[rat+2])*0.5*2.7,
    00226         cos(ratios[rat+2])*0.5*300*cos(dir*pi_4),
    00227         cos(ratios[rat+2])*0.5*300*sin(dir*pi_4));
    00228       withWalk[1][rat+2][dir+4].correctedMotion=withWalk[1][rat+2][dir+4].requestedMotion;
    00229
    00230       withWalk[0][rat+2][dir+4]=msh7; //invEvo; //invQU;
    00231       withWalk[0][rat+2][dir+4].index=7+5*8*0+8*(rat+2)+(dir+4);
    00232       withWalk[0][rat+2][dir+4].requestedMotion=Pose2D(
    00233         sin(ratios[rat+2])*0.2*2.7,
    00234         cos(ratios[rat+2])*0.2*300*cos(dir*pi_4),
    00235         cos(ratios[rat+2])*0.2*300*sin(dir*pi_4));
    00236       withWalk[0][rat+2][dir+4].correctedMotion=withWalk[0][rat+2][dir+4].requestedMotion;
    00237     }
    00238   }
    00239
    00240   withWalk[2][2][4]=msh7; //ers7fastforward;
    00241   withWalk[2][2][4].index=7+5*8*2+8*2+4;
    00242   withWalk[2][2][4].requestedMotion=Pose2D(0, 400,0);
    00243   withWalk[2][2][4].correctedMotion=withWalk[2][2][4].requestedMotion;
    00244
    00245   withWalk[1][2][4].requestedMotion=Pose2D(0, 250,0);
    00246   withWalk[1][2][4].correctedMotion=withWalk[1][2][4].requestedMotion;
    00247
    00248   withWalk[2][2][0]=msh7backward;
    00249   withWalk[2][2][0].index=7+5*8*2+8*2+0;
    00250   withWalk[2][2][0].requestedMotion=Pose2D(0, -294,0);
    00251   withWalk[2][2][0].correctedMotion=withWalk[2][2][0].requestedMotion;
    00252
    00253   mergedParameters=&rotationOnly[3];
    00254
    00255   //if there is an parametersSet for this robotDesign, then load it:
    00256   load();
    00257 }
    00258
    00259 void GT2004ParametersSet::mirrorLeftTurnToRightTurn()
    00260 {
    00261   mirrorThis(few_lturn_0_fast);
    00262   mirrorThis(few_lturn_min180_fast);
    00263   mirrorThis(few_lturn_45_fast);
    00264   mirrorThis(few_lturn_min135_fast);
    00265   mirrorThis(few_lturn_90_fast);
    00266   mirrorThis(few_lturn_min90_fast);
    00267   mirrorThis(few_lturn_135_fast);
    00268   mirrorThis(few_lturn_min45_fast);
    00269
    00270   mirrorThis(few_lturn_0_med);
    00271   mirrorThis(few_lturn_min180_med);
    00272   mirrorThis(few_lturn_45_med);
    00273   mirrorThis(few_lturn_min135_med);
    00274   mirrorThis(few_lturn_90_med);
    00275   mirrorThis(few_lturn_min90_med);
    00276   mirrorThis(few_lturn_135_med);
    00277   mirrorThis(few_lturn_min45_med);
    00278
    00279   mirrorThis(few_lturn_0_slow);
    00280   mirrorThis(few_lturn_min180_slow);
    00281   mirrorThis(few_lturn_45_slow);
    00282   mirrorThis(few_lturn_min135_slow);
    00283   mirrorThis(few_lturn_90_slow);
    00284   mirrorThis(few_lturn_min90_slow);
    00285   mirrorThis(few_lturn_135_slow);
    00286   mirrorThis(few_lturn_min45_slow);
    00287
    00288   mirrorThis(med_lturn_0_fast);
    00289   mirrorThis(med_lturn_min180_fast);
    00290   mirrorThis(med_lturn_45_fast);
    00291   mirrorThis(med_lturn_min135_fast);
    00292   mirrorThis(med_lturn_90_fast);
    00293   mirrorThis(med_lturn_min90_fast);
    00294   mirrorThis(med_lturn_135_fast);
    00295   mirrorThis(med_lturn_min45_fast);
    00296
    00297   mirrorThis(med_lturn_0_med);
    00298   mirrorThis(med_lturn_min180_med);
    00299   mirrorThis(med_lturn_45_med);
    00300   mirrorThis(med_lturn_min135_med);
    00301   mirrorThis(med_lturn_90_med);
    00302   mirrorThis(med_lturn_min90_med);
    00303   mirrorThis(med_lturn_135_med);
    00304   mirrorThis(med_lturn_min45_med);
    00305
    00306   mirrorThis(med_lturn_0_slow);
    00307   mirrorThis(med_lturn_min180_slow);
    00308   mirrorThis(med_lturn_45_slow);
    00309   mirrorThis(med_lturn_min135_slow);
    00310   mirrorThis(med_lturn_90_slow);
    00311   mirrorThis(med_lturn_min90_slow);
    00312   mirrorThis(med_lturn_135_slow);
    00313   mirrorThis(med_lturn_min45_slow);
    00314
    00315   mirrorThis(much_lturn_slow);
    00316   mirrorThis(much_lturn_med);
    00317   mirrorThis(much_lturn_fast);
    00318 }
    00319
    00320 void GT2004ParametersSet::calculateMergedParameterSet(Pose2D& currentRequest)
    00321 {
    00322   if ((currentRequest.rotation==0)&&(currentRequest.translation.abs()==0))
    00323   {
    00324     mergedParameters=&rotationOnly[3];
    00325   }
    00326   else
    00327   {
    00328     double rotationWalkRatio=atan2(currentRequest.rotation/2.7,currentRequest.translation.abs()/300)*2/pi;
    00329     double movementStrength=sqrt(sqr(currentRequest.translation.abs()/300)+sqr(currentRequest.rotation/2.7));
    00330
    00331     //determine in which turn/walk ratio segment we are:
    00332     int lowerRatioIndex;
    00333     double lowerRatioFactor;
    00334     if (rotationWalkRatio<0)
    00335     {
    00336       if (rotationWalkRatio<-0.3)
    00337       {
    00338         lowerRatioIndex=0;
    00339         lowerRatioFactor=(rotationWalkRatio+0.3)/(-1.0+0.3);
    00340       }
    00341       else
    00342       {
    00343         if (currentRequest.rotation<-0.1)
    00344         {
    00345           lowerRatioIndex=1;
    00346           lowerRatioFactor=(rotationWalkRatio+0.1)/(-0.3+0.1);
    00347         }
    00348         else
    00349         {
    00350           lowerRatioIndex=2;
    00351           lowerRatioFactor=rotationWalkRatio/(-0.1);
    00352         }
    00353       }
    00354     }
    00355     else
    00356     {
    00357       if (rotationWalkRatio<0.3)
    00358       {
    00359         if (rotationWalkRatio<0.1)
    00360         {
    00361           lowerRatioIndex=3;
    00362           lowerRatioFactor=(0.1-rotationWalkRatio)/(0.1);
    00363         }
    00364         else
    00365         {
    00366           lowerRatioIndex=4;
    00367           lowerRatioFactor=(0.3-rotationWalkRatio)/(0.3-0.1);
    00368         }
    00369       }
    00370       else
    00371       {
    00372         lowerRatioIndex=5;
    00373         lowerRatioFactor=(1.0-rotationWalkRatio)/(1.0-0.3);
    00374       }
    00375     }
    00376
    00377     //determine in which walk direction segment we are
    00378     double direction=atan2(currentRequest.translation.y,currentRequest.translation.x);
    00379     if (direction==pi) { direction=-pi; }
    00380     int lowerDirectionIndex=(int)((direction/pi+1)*4);
    00381     double lowerDirectionFactor= 1.0-((direction/pi+1)*4-lowerDirectionIndex);
    00382
    00383     //calculate the maximum movement strength into the requested direction with requested turn/walk ratio
    00384     double lowerRatioMaxStrength=(lowerRatioIndex==0)?fabs(rotationOnly[0].requestedMotion.rotation)/2.7:
    00385     sqrt(
    00386       sqr(withWalk[2][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
    00387       sqr(withWalk[2][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
    00388       sqrt(
    00389       sqr(withWalk[2][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
    00390       sqr(withWalk[2][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
    00391
    00392     double upperRatioMaxStrength=(lowerRatioIndex>=5)?fabs(rotationOnly[6].requestedMotion.rotation)/2.7:
    00393     sqrt(
    00394       sqr(withWalk[2][lowerRatioIndex][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
    00395       sqr(withWalk[2][lowerRatioIndex][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
    00396       sqrt(
    00397       sqr(withWalk[2][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
    00398       sqr(withWalk[2][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
    00399     double maxStrength=lowerRatioMaxStrength*lowerRatioFactor + upperRatioMaxStrength*(1.0-lowerRatioFactor);
    00400
    00401     //determine in which speed segment we are
    00402     int lowerSpeedIndex;
    00403     double lowerSpeedFactor;
    00404     if (movementStrength>=maxStrength)
    00405     {
    00406       //restrict requested motion to maximum strength with that direction and turn/walk ratio
    00407       currentRequest.translation *= maxStrength/movementStrength;
    00408       currentRequest.rotation *= maxStrength/movementStrength;
    00409       movementStrength=maxStrength;
    00410       lowerSpeedIndex=3;
    00411       lowerSpeedFactor=1;
    00412     }
    00413     else
    00414     {
    00415       //calculation of medStrength
    00416       double lowerRatioMedStrength=(lowerRatioIndex==0)?fabs(rotationOnly[1].requestedMotion.rotation)/2.7:
    00417       sqrt(
    00418         sqr(withWalk[1][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
    00419         sqr(withWalk[1][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
    00420         sqrt(
    00421         sqr(withWalk[1][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
    00422         sqr(withWalk[1][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
    00423
    00424       double upperRatioMedStrength=(lowerRatioIndex>=5)?fabs(rotationOnly[5].requestedMotion.rotation)/2.7:
    00425       sqrt(
    00426         sqr(withWalk[1][lowerRatioIndex][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
    00427         sqr(withWalk[1][lowerRatioIndex][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
    00428         sqrt(
    00429         sqr(withWalk[1][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
    00430         sqr(withWalk[1][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
    00431       double medStrength=lowerRatioMedStrength*lowerRatioFactor + upperRatioMedStrength*(1.0-lowerRatioFactor);
    00432
    00433       if (movementStrength>=medStrength)
    00434       {
    00435         lowerSpeedIndex=2;
    00436         lowerSpeedFactor=(maxStrength-movementStrength)/(maxStrength-medStrength);
    00437       }
    00438       else
    00439       {
    00440         //calculation of minStrength
    00441         double lowerRatioMinStrength=(lowerRatioIndex==0)?fabs(rotationOnly[2].requestedMotion.rotation)/2.7:
    00442         sqrt(
    00443           sqr(withWalk[0][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
    00444           sqr(withWalk[0][lowerRatioIndex-1][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
    00445           sqrt(
    00446           sqr(withWalk[0][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
    00447           sqr(withWalk[0][lowerRatioIndex-1][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
    00448
    00449         double upperRatioMinStrength=(lowerRatioIndex>=5)?fabs(rotationOnly[4].requestedMotion.rotation)/2.7:
    00450         sqrt(
    00451           sqr(withWalk[0][lowerRatioIndex][lowerDirectionIndex].requestedMotion.translation.abs()/300)+
    00452           sqr(withWalk[0][lowerRatioIndex][lowerDirectionIndex].requestedMotion.rotation/2.7))*lowerDirectionFactor+
    00453           sqrt(
    00454           sqr(withWalk[0][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.translation.abs()/300)+
    00455           sqr(withWalk[0][lowerRatioIndex][(lowerDirectionIndex+1)%8].requestedMotion.rotation/2.7))*(1.0-lowerDirectionFactor);
    00456         double minStrength=lowerRatioMinStrength*lowerRatioFactor + upperRatioMinStrength*(1.0-lowerRatioFactor);
    00457         if (movementStrength>=minStrength)
    00458         {
    00459           lowerSpeedIndex=1;
    00460           lowerSpeedFactor=(medStrength-movementStrength)/(medStrength-minStrength);
    00461         }
    00462         else
    00463         {
    00464           lowerSpeedIndex=0;
    00465           lowerSpeedFactor=(minStrength-movementStrength)/(minStrength-0);
    00466         }
    00467       }
    00468     }
    00469
    00470     //calculate merged parameter set(s) with lower/upperRatio/Direction/SpeedIndex/Factor
    00471     if (lowerRatioFactor>0)
    00472     {
    00473       //calculate merged parameter set for lower ratio and lower speed
    00474       if (lowerSpeedIndex==0)
    00475       {
    00476         lowRatioLowSpeed=&rotationOnly[3];
    00477       }
    00478       else if (lowerRatioIndex==0)
    00479       {
    00480         //only turn right
    00481         lowRatioLowSpeed=&rotationOnly[3-lowerSpeedIndex];
    00482       }
    00483       else
    00484       {
    00485         if (lowerDirectionFactor==1)
    00486         {
    00487           lowRatioLowSpeed=&withWalk[lowerSpeedIndex-1][lowerRatioIndex-1][lowerDirectionIndex];
    00488         }
    00489         else
    00490         {
    00491           lowRatioLowSpeed=&llBuf;
    00492           lowRatioLowSpeed->interpolate(
    00493             withWalk[lowerSpeedIndex-1][lowerRatioIndex-1][lowerDirectionIndex],
    00494             withWalk[lowerSpeedIndex-1][lowerRatioIndex-1][(lowerDirectionIndex+1)%8],
    00495             lowerDirectionFactor);
    00496         }
    00497       }
    00498
    00499       //calculate merged parameter set for lower ratio and upper speed
    00500       if (lowerSpeedIndex==3)
    00501       {
    00502         lowRatioUpSpeed=lowRatioLowSpeed;
    00503       }
    00504       else if (lowerRatioIndex==0)
    00505       {
    00506         lowRatioUpSpeed=&rotationOnly[3-min(lowerSpeedIndex+1,3)];
    00507       }
    00508       else
    00509       {
    00510         if (lowerDirectionFactor==1)
    00511         {
    00512           lowRatioUpSpeed=&withWalk[lowerSpeedIndex][lowerRatioIndex-1][lowerDirectionIndex];
    00513         }
    00514         else
    00515         {
    00516           lowRatioUpSpeed=&luBuf;
    00517           lowRatioUpSpeed->interpolate(
    00518             withWalk[lowerSpeedIndex][lowerRatioIndex-1][lowerDirectionIndex],
    00519             withWalk[lowerSpeedIndex][lowerRatioIndex-1][(lowerDirectionIndex+1)%8],
    00520             lowerDirectionFactor);
    00521         }
    00522       }
    00523     }
    00524
    00525     if (lowerRatioFactor<1)
    00526     {
    00527       //calculate merged parameter set for upper ratio and lower speed
    00528       if (lowerRatioIndex>=5)
    00529       {
    00530         //only turn left
    00531         upRatioLowSpeed=&rotationOnly[3+lowerSpeedIndex];
    00532       }
    00533       else if (lowerSpeedIndex==0)
    00534       {
    00535         upRatioLowSpeed=&rotationOnly[3];
    00536       }
    00537       else
    00538       {
    00539         if (lowerDirectionFactor==1)
    00540         {
    00541           upRatioLowSpeed=&withWalk[lowerSpeedIndex-1][lowerRatioIndex][lowerDirectionIndex];
    00542         }
    00543         else
    00544         {
    00545           upRatioLowSpeed=&ulBuf;
    00546           upRatioLowSpeed->interpolate(
    00547             withWalk[lowerSpeedIndex-1][lowerRatioIndex][lowerDirectionIndex],
    00548             withWalk[lowerSpeedIndex-1][lowerRatioIndex][(lowerDirectionIndex+1)%8],
    00549             lowerDirectionFactor);
    00550         }
    00551       }
    00552
    00553       //calculate merged parameter set for upper ratio and upper speed
    00554       if (lowerRatioIndex>=5)
    00555       {
    00556         //only turn left
    00557         upRatioUpSpeed=&rotationOnly[3+min(lowerSpeedIndex+1,3)];
    00558       }
    00559       else if (lowerSpeedIndex==3)
    00560       {
    00561         upRatioUpSpeed=upRatioLowSpeed;
    00562       }
    00563       else
    00564       {
    00565         if (lowerDirectionFactor==1)
    00566         {
    00567           upRatioUpSpeed=&withWalk[lowerSpeedIndex][lowerRatioIndex][lowerDirectionIndex];
    00568         }
    00569         else
    00570         {
    00571           upRatioUpSpeed=&uuBuf;
    00572           upRatioUpSpeed->interpolate(
    00573             withWalk[lowerSpeedIndex][lowerRatioIndex][lowerDirectionIndex],
    00574             withWalk[lowerSpeedIndex][lowerRatioIndex][(lowerDirectionIndex+1)%8],
    00575             lowerDirectionFactor);
    00576         }
    00577       }
    00578     }
    00579
    00580     //calculate merged parameter sets for lower and upper speed
    00581     if (lowerRatioFactor==1)
    00582     {
    00583       lowSpeed=lowRatioLowSpeed;
    00584       upSpeed=lowRatioUpSpeed;
    00585     }
    00586     else if (lowerRatioFactor==0)
    00587     {
    00588       lowSpeed=upRatioLowSpeed;
    00589       upSpeed=upRatioUpSpeed;
    00590     }
    00591     else
    00592     {
    00593       lowSpeed=&lBuf;
    00594       lowSpeed->interpolate(*lowRatioLowSpeed,*upRatioLowSpeed,lowerRatioFactor);
    00595       if (lowerSpeedFactor==1)
    00596       {
    00597         upSpeed=lowSpeed;
    00598       }
    00599       else
    00600       {
    00601         upSpeed=&uBuf;
    00602         upSpeed->interpolate(*lowRatioUpSpeed,*upRatioUpSpeed,lowerRatioFactor);
    00603       }
    00604     }
    00605
    00606     //calculate finally merged parameter set
    00607     if (lowerSpeedFactor==1)
    00608     {
    00609       mergedParameters=lowSpeed;
    00610     }
    00611     else
    00612     {
    00613       mergedParameters=&mBuf;
    00614       mergedParameters->interpolate(*lowSpeed,*upSpeed,lowerSpeedFactor);
    00615     }
    00616   }
    00617 }
    00618
    00619 GT2004Parameters* GT2004ParametersSet::getParameters(int index)
    00620 {
    00621   if (index<7)
    00622   {
    00623     if (index<0){ index=0; }
    00624     return &rotationOnly[index];
    00625   }
    00626   else
    00627   {
    00628     if (index>=7+3*5*8){ index=7+3*5*8-1; }
    00629     int direction=(index-7)%8;
    00630     int ratio=((index-7)/8)%5;
    00631     int speed=(index-7)/40;
    00632     return &withWalk[speed][ratio][direction];
    00633   }
    00634 }
    00635 bool GT2004ParametersSet::isMaxSpeedIndex(int index)
    00636 {
    00637   if (index<7)
    00638   {
    00639     if (index<0){ index=0; }
    00640     return ((index==0)||(index==6));
    00641   }
    00642   else
    00643   {
    00644     if (index>=7+3*5*8){ index=7+3*5*8-1; }
    00645     int speed=(index-7)/40;
    00646     return (speed==2);
    00647   }
    00648 }
    00649
    00650 char* GT2004ParametersSet::getIndexString(int index)
    00651 {
    00652   switch ((IndexName)index)
    00653   {
    00654   case much_lturn_fast: return "much_lturn_fast"; break;
    00655   case much_lturn_med: return "much_lturn_med"; break;
    00656   case much_lturn_slow: return "much_lturn_slow"; break;
    00657   case stand: return "stand"; break;
    00658   case much_rturn_slow: return "much_rturn_slow"; break;
    00659   case much_rturn_med: return "much_rturn_med"; break;
    00660   case much_rturn_fast: return "much_rturn_fast"; break;
    00661   case med_lturn_min180_slow: return "med_lturn_min180_slow"; break;
    00662   case med_lturn_min135_slow: return "med_lturn_min135_slow"; break;
    00663   case med_lturn_min90_slow: return "med_lturn_min90_slow"; break;
    00664   case med_lturn_min45_slow: return "med_lturn_min45_slow"; break;
    00665   case med_lturn_0_slow: return "med_lturn_0_slow"; break;
    00666   case med_lturn_45_slow: return "med_lturn_45_slow"; break;
    00667   case med_lturn_90_slow: return "med_lturn_90_slow"; break;
    00668   case med_lturn_135_slow: return "med_lturn_135_slow"; break;
    00669   case few_lturn_min180_slow: return "few_lturn_min180_slow"; break;
    00670   case few_lturn_min135_slow: return "few_lturn_min135_slow"; break;
    00671   case few_lturn_min90_slow: return "few_lturn_min90_slow"; break;
    00672   case few_lturn_min45_slow: return "few_lturn_min45_slow"; break;
    00673   case few_lturn_0_slow: return "few_lturn_0_slow"; break;
    00674   case few_lturn_45_slow: return "few_lturn_45_slow"; break;
    00675   case few_lturn_90_slow: return "few_lturn_90_slow"; break;
    00676   case few_lturn_135_slow: return "few_lturn_135_slow"; break;
    00677   case no_turn_min180_slow: return "no_turn_min180_slow"; break;
    00678   case no_turn_min135_slow: return "no_turn_min135_slow"; break;
    00679   case no_turn_min90_slow: return "no_turn_min90_slow"; break;
    00680   case no_turn_min45_slow: return "no_turn_min45_slow"; break;
    00681   case no_turn_0_slow: return "no_turn_0_slow"; break;
    00682   case no_turn_45_slow: return "no_turn_45_slow"; break;
    00683   case no_turn_90_slow: return "no_turn_90_slow"; break;
    00684   case no_turn_135_slow: return "no_turn_135_slow"; break;
    00685   case few_rturn_min180_slow: return "few_rturn_min180_slow"; break;
    00686   case few_rturn_min135_slow: return "few_rturn_min135_slow"; break;
    00687   case few_rturn_min90_slow: return "few_rturn_min90_slow"; break;
    00688   case few_rturn_min45_slow: return "few_rturn_min45_slow"; break;
    00689   case few_rturn_0_slow: return "few_rturn_0_slow"; break;
    00690   case few_rturn_45_slow: return "few_rturn_45_slow"; break;
    00691   case few_rturn_90_slow: return "few_rturn_90_slow"; break;
    00692   case few_rturn_135_slow: return "few_rturn_135_slow"; break;
    00693   case med_rturn_min180_slow: return "med_rturn_min180_slow"; break;
    00694   case med_rturn_min135_slow: return "med_rturn_min135_slow"; break;
    00695   case med_rturn_min90_slow: return "med_rturn_min90_slow"; break;
    00696   case med_rturn_min45_slow: return "med_rturn_min45_slow"; break;
    00697   case med_rturn_0_slow: return "med_rturn_0_slow"; break;
    00698   case med_rturn_45_slow: return "med_rturn_45_slow"; break;
    00699   case med_rturn_90_slow: return "med_rturn_90_slow"; break;
    00700   case med_rturn_135_slow: return "med_rturn_135_slow"; break;
    00701   case med_lturn_min180_med: return "med_lturn_min180_med"; break;
    00702   case med_lturn_min135_med: return "med_lturn_min135_med"; break;
    00703   case med_lturn_min90_med: return "med_lturn_min90_med"; break;
    00704   case med_lturn_min45_med: return "med_lturn_min45_med"; break;
    00705   case med_lturn_0_med: return "med_lturn_0_med"; break;
    00706   case med_lturn_45_med: return "med_lturn_45_med"; break;
    00707   case med_lturn_90_med: return "med_lturn_90_med"; break;
    00708   case med_lturn_135_med: return "med_lturn_135_med"; break;
    00709   case few_lturn_min180_med: return "few_lturn_min180_med"; break;
    00710   case few_lturn_min135_med: return "few_lturn_min135_med"; break;
    00711   case few_lturn_min90_med: return "few_lturn_min90_med"; break;
    00712   case few_lturn_min45_med: return "few_lturn_min45_med"; break;
    00713   case few_lturn_0_med: return "few_lturn_0_med"; break;
    00714   case few_lturn_45_med: return "few_lturn_45_med"; break;
    00715   case few_lturn_90_med: return "few_lturn_90_med"; break;
    00716   case few_lturn_135_med: return "few_lturn_135_med"; break;
    00717   case no_turn_min180_med: return "no_turn_min180_med"; break;
    00718   case no_turn_min135_med: return "no_turn_min135_med"; break;
    00719   case no_turn_min90_med: return "no_turn_min90_med"; break;
    00720   case no_turn_min45_med: return "no_turn_min45_med"; break;
    00721   case no_turn_0_med: return "no_turn_0_med"; break;
    00722   case no_turn_45_med: return "no_turn_45_med"; break;
    00723   case no_turn_90_med: return "no_turn_90_med"; break;
    00724   case no_turn_135_med: return "no_turn_135_med"; break;
    00725   case few_rturn_min180_med: return "few_rturn_min180_med"; break;
    00726   case few_rturn_min135_med: return "few_rturn_min135_med"; break;
    00727   case few_rturn_min90_med: return "few_rturn_min90_med"; break;
    00728   case few_rturn_min45_med: return "few_rturn_min45_med"; break;
    00729   case few_rturn_0_med: return "few_rturn_0_med"; break;
    00730   case few_rturn_45_med: return "few_rturn_45_med"; break;
    00731   case few_rturn_90_med: return "few_rturn_90_med"; break;
    00732   case few_rturn_135_med: return "few_rturn_135_med"; break;
    00733   case med_rturn_min180_med: return "med_rturn_min180_med"; break;
    00734   case med_rturn_min135_med: return "med_rturn_min135_med"; break;
    00735   case med_rturn_min90_med: return "med_rturn_min90_med"; break;
    00736   case med_rturn_min45_med: return "med_rturn_min45_med"; break;
    00737   case med_rturn_0_med: return "med_rturn_0_med"; break;
    00738   case med_rturn_45_med: return "med_rturn_45_med"; break;
    00739   case med_rturn_90_med: return "med_rturn_90_med"; break;
    00740   case med_rturn_135_med: return "med_rturn_135_med"; break;
    00741   case med_lturn_min180_fast: return "med_lturn_min180_fast"; break;
    00742   case med_lturn_min135_fast: return "med_lturn_min135_fast"; break;
    00743   case med_lturn_min90_fast: return "med_lturn_min90_fast"; break;
    00744   case med_lturn_min45_fast: return "med_lturn_min45_fast"; break;
    00745   case med_lturn_0_fast: return "med_lturn_0_fast"; break;
    00746   case med_lturn_45_fast: return "med_lturn_45_fast"; break;
    00747   case med_lturn_90_fast: return "med_lturn_90_fast"; break;
    00748   case med_lturn_135_fast: return "med_lturn_135_fast"; break;
    00749   case few_lturn_min180_fast: return "few_lturn_min180_fast"; break;
    00750   case few_lturn_min135_fast: return "few_lturn_min135_fast"; break;
    00751   case few_lturn_min90_fast: return "few_lturn_min90_fast"; break;
    00752   case few_lturn_min45_fast: return "few_lturn_min45_fast"; break;
    00753   case few_lturn_0_fast: return "few_lturn_0_fast"; break;
    00754   case few_lturn_45_fast: return "few_lturn_45_fast"; break;
    00755   case few_lturn_90_fast: return "few_lturn_90_fast"; break;
    00756   case few_lturn_135_fast: return "few_lturn_135_fast"; break;
    00757   case no_turn_min180_fast: return "no_turn_min180_fast"; break;
    00758   case no_turn_min135_fast: return "no_turn_min135_fast"; break;
    00759   case no_turn_min90_fast: return "no_turn_min90_fast"; break;
    00760   case no_turn_min45_fast: return "no_turn_min45_fast"; break;
    00761   case no_turn_0_fast: return "no_turn_0_fast"; break;
    00762   case no_turn_45_fast: return "no_turn_45_fast"; break;
    00763   case no_turn_90_fast: return "no_turn_90_fast"; break;
    00764   case no_turn_135_fast: return "no_turn_135_fast"; break;
    00765   case few_rturn_min180_fast: return "few_rturn_min180_fast"; break;
    00766   case few_rturn_min135_fast: return "few_rturn_min135_fast"; break;
    00767   case few_rturn_min90_fast: return "few_rturn_min90_fast"; break;
    00768   case few_rturn_min45_fast: return "few_rturn_min45_fast"; break;
    00769   case few_rturn_0_fast: return "few_rturn_0_fast"; break;
    00770   case few_rturn_45_fast: return "few_rturn_45_fast"; break;
    00771   case few_rturn_90_fast: return "few_rturn_90_fast"; break;
    00772   case few_rturn_135_fast: return "few_rturn_135_fast"; break;
    00773   case med_rturn_min180_fast: return "med_rturn_min180_fast"; break;
    00774   case med_rturn_min135_fast: return "med_rturn_min135_fast"; break;
    00775   case med_rturn_min90_fast: return "med_rturn_min90_fast"; break;
    00776   case med_rturn_min45_fast: return "med_rturn_min45_fast"; break;
    00777   case med_rturn_0_fast: return "med_rturn_0_fast"; break;
    00778   case med_rturn_45_fast: return "med_rturn_45_fast"; break;
    00779   case med_rturn_90_fast: return "med_rturn_90_fast"; break;
    00780   case med_rturn_135_fast: return "med_rturn_135_fast"; break;
    00781   default: return "undefined";
    00782   }
    00783 }
    00784
    00785 int GT2004ParametersSet::getIndexOfMirror(int index)
    00786 {
    00787   switch ((IndexName)index)
    00788   {
    00789     //turn only:
    00790   case much_lturn_fast: return (int)much_rturn_fast; break;
    00791   case much_lturn_med: return (int)much_rturn_med; break;
    00792   case much_lturn_slow: return (int)much_rturn_slow; break;
    00793   case much_rturn_slow: return (int)much_lturn_slow; break;
    00794   case much_rturn_med: return (int)much_lturn_med; break;
    00795   case much_rturn_fast: return (int)much_lturn_fast; break;
    00796     //180:
    00797   case med_lturn_min180_slow: return (int)med_rturn_min180_slow; break;
    00798   case few_lturn_min180_slow: return (int)few_rturn_min180_slow; break;
    00799   case few_rturn_min180_slow: return (int)few_lturn_min180_slow; break;
    00800   case med_rturn_min180_slow: return (int)med_lturn_min180_slow; break;
    00801   case med_lturn_min180_med: return (int)med_rturn_min180_med; break;
    00802   case few_lturn_min180_med: return (int)few_rturn_min180_med; break;
    00803   case few_rturn_min180_med: return (int)few_lturn_min180_med; break;
    00804   case med_rturn_min180_med: return (int)med_lturn_min180_med; break;
    00805   case med_lturn_min180_fast: return (int)med_rturn_min180_fast; break;
    00806   case few_lturn_min180_fast: return (int)few_rturn_min180_fast; break;
    00807   case few_rturn_min180_fast: return (int)few_lturn_min180_fast; break;
    00808   case med_rturn_min180_fast: return (int)med_lturn_min180_fast; break;
    00809     //0:
    00810   case med_lturn_0_slow: return (int)med_rturn_0_slow; break;
    00811   case few_lturn_0_slow: return (int)few_rturn_0_slow; break;
    00812   case few_rturn_0_slow: return (int)few_lturn_0_slow; break;
    00813   case med_rturn_0_slow: return (int)med_lturn_0_slow; break;
    00814   case med_lturn_0_med: return (int)med_rturn_0_med; break;
    00815   case few_lturn_0_med: return (int)few_rturn_0_med; break;
    00816   case few_rturn_0_med: return (int)few_lturn_0_med; break;
    00817   case med_rturn_0_med: return (int)med_lturn_0_med; break;
    00818   case med_lturn_0_fast: return (int)med_rturn_0_fast; break;
    00819   case few_lturn_0_fast: return (int)few_rturn_0_fast; break;
    00820   case few_rturn_0_fast: return (int)few_lturn_0_fast; break;
    00821   case med_rturn_0_fast: return (int)med_lturn_0_fast; break;
    00822     //+-135 slow:
    00823   case med_lturn_min135_slow: return (int)med_rturn_135_slow; break;
    00824   case few_lturn_min135_slow: return (int)few_rturn_135_slow; break;
    00825   case no_turn_min135_slow: return (int)no_turn_135_slow; break;
    00826   case few_rturn_min135_slow: return (int)few_lturn_135_slow; break;
    00827   case med_rturn_min135_slow: return (int)med_lturn_135_slow; break;
    00828   case med_lturn_135_slow: return (int)med_rturn_min135_slow; break;
    00829   case few_lturn_135_slow: return (int)few_rturn_min135_slow; break;
    00830   case no_turn_135_slow: return (int)no_turn_min135_slow; break;
    00831   case few_rturn_135_slow: return (int)few_lturn_min135_slow; break;
    00832   case med_rturn_135_slow: return (int)med_lturn_min135_slow; break;
    00833     //+-90 slow:
    00834   case med_lturn_min90_slow: return (int)med_rturn_90_slow; break;
    00835   case few_lturn_min90_slow: return (int)few_rturn_90_slow; break;
    00836   case no_turn_min90_slow: return (int)no_turn_90_slow; break;
    00837   case few_rturn_min90_slow: return (int)few_lturn_90_slow; break;
    00838   case med_rturn_min90_slow: return (int)med_lturn_90_slow; break;
    00839   case med_lturn_90_slow: return (int)med_rturn_min90_slow; break;
    00840   case few_lturn_90_slow: return (int)few_rturn_min90_slow; break;
    00841   case no_turn_90_slow: return (int)no_turn_min90_slow; break;
    00842   case few_rturn_90_slow: return (int)few_lturn_min90_slow; break;
    00843   case med_rturn_90_slow: return (int)med_lturn_min90_slow; break;
    00844     //+-45 slow:
    00845   case med_lturn_min45_slow: return (int)med_rturn_45_slow; break;
    00846   case few_lturn_min45_slow: return (int)few_rturn_45_slow; break;
    00847   case no_turn_min45_slow: return (int)no_turn_45_slow; break;
    00848   case few_rturn_min45_slow: return (int)few_lturn_45_slow; break;
    00849   case med_rturn_min45_slow: return (int)med_lturn_45_slow; break;
    00850   case med_lturn_45_slow: return (int)med_rturn_min45_slow; break;
    00851   case few_lturn_45_slow: return (int)few_rturn_min45_slow; break;
    00852   case no_turn_45_slow: return (int)no_turn_min45_slow; break;
    00853   case few_rturn_45_slow: return (int)few_lturn_min45_slow; break;
    00854   case med_rturn_45_slow: return (int)med_lturn_min45_slow; break;
    00855     //+-135 med:
    00856   case med_lturn_min135_med: return (int)med_rturn_135_med; break;
    00857   case few_lturn_min135_med: return (int)few_rturn_135_med; break;
    00858   case no_turn_min135_med: return (int)no_turn_135_med; break;
    00859   case few_rturn_min135_med: return (int)few_lturn_135_med; break;
    00860   case med_rturn_min135_med: return (int)med_lturn_135_med; break;
    00861   case med_lturn_135_med: return (int)med_rturn_min135_med; break;
    00862   case few_lturn_135_med: return (int)few_rturn_min135_med; break;
    00863   case no_turn_135_med: return (int)no_turn_min135_med; break;
    00864   case few_rturn_135_med: return (int)few_lturn_min135_med; break;
    00865   case med_rturn_135_med: return (int)med_lturn_min135_med; break;
    00866     //+-90 med:
    00867   case med_lturn_min90_med: return (int)med_rturn_90_med; break;
    00868   case few_lturn_min90_med: return (int)few_rturn_90_med; break;
    00869   case no_turn_min90_med: return (int)no_turn_90_med; break;
    00870   case few_rturn_min90_med: return (int)few_lturn_90_med; break;
    00871   case med_rturn_min90_med: return (int)med_lturn_90_med; break;
    00872   case med_lturn_90_med: return (int)med_rturn_min90_med; break;
    00873   case few_lturn_90_med: return (int)few_rturn_min90_med; break;
    00874   case no_turn_90_med: return (int)no_turn_min90_med; break;
    00875   case few_rturn_90_med: return (int)few_lturn_min90_med; break;
    00876   case med_rturn_90_med: return (int)med_lturn_min90_med; break;
    00877     //+-45 med:
    00878   case med_lturn_min45_med: return (int)med_rturn_45_med; break;
    00879   case few_lturn_min45_med: return (int)few_rturn_45_med; break;
    00880   case no_turn_min45_med: return (int)no_turn_45_med; break;
    00881   case few_rturn_min45_med: return (int)few_lturn_45_med; break;
    00882   case med_rturn_min45_med: return (int)med_lturn_45_med; break;
    00883   case med_lturn_45_med: return (int)med_rturn_min45_med; break;
    00884   case few_lturn_45_med: return (int)few_rturn_min45_med; break;
    00885   case no_turn_45_med: return (int)no_turn_min45_med; break;
    00886   case few_rturn_45_med: return (int)few_lturn_min45_med; break;
    00887   case med_rturn_45_med: return (int)med_lturn_min45_med; break;
    00888     //+-135 fast:
    00889   case med_lturn_min135_fast: return (int)med_rturn_135_fast; break;
    00890   case few_lturn_min135_fast: return (int)few_rturn_135_fast; break;
    00891   case no_turn_min135_fast: return (int)no_turn_135_fast; break;
    00892   case few_rturn_min135_fast: return (int)few_lturn_135_fast; break;
    00893   case med_rturn_min135_fast: return (int)med_lturn_135_fast; break;
    00894   case med_lturn_135_fast: return (int)med_rturn_min135_fast; break;
    00895   case few_lturn_135_fast: return (int)few_rturn_min135_fast; break;
    00896   case no_turn_135_fast: return (int)no_turn_min135_fast; break;
    00897   case few_rturn_135_fast: return (int)few_lturn_min135_fast; break;
    00898   case med_rturn_135_fast: return (int)med_lturn_min135_fast; break;
    00899     //+-90 fast:
    00900   case med_lturn_min90_fast: return (int)med_rturn_90_fast; break;
    00901   case few_lturn_min90_fast: return (int)few_rturn_90_fast; break;
    00902   case no_turn_min90_fast: return (int)no_turn_90_fast; break;
    00903   case few_rturn_min90_fast: return (int)few_lturn_90_fast; break;
    00904   case med_rturn_min90_fast: return (int)med_lturn_90_fast; break;
    00905   case med_lturn_90_fast: return (int)med_rturn_min90_fast; break;
    00906   case few_lturn_90_fast: return (int)few_rturn_min90_fast; break;
    00907   case no_turn_90_fast: return (int)no_turn_min90_fast; break;
    00908   case few_rturn_90_fast: return (int)few_lturn_min90_fast; break;
    00909   case med_rturn_90_fast: return (int)med_lturn_min90_fast; break;
    00910     //+-45 fast:
    00911   case med_lturn_min45_fast: return (int)med_rturn_45_fast; break;
    00912   case few_lturn_min45_fast: return (int)few_rturn_45_fast; break;
    00913   case no_turn_min45_fast: return (int)no_turn_45_fast; break;
    00914   case few_rturn_min45_fast: return (int)few_lturn_45_fast; break;
    00915   case med_rturn_min45_fast: return (int)med_lturn_45_fast; break;
    00916   case med_lturn_45_fast: return (int)med_rturn_min45_fast; break;
    00917   case few_lturn_45_fast: return (int)few_rturn_min45_fast; break;
    00918   case no_turn_45_fast: return (int)no_turn_min45_fast; break;
    00919   case few_rturn_45_fast: return (int)few_lturn_min45_fast; break;
    00920   case med_rturn_45_fast: return (int)med_lturn_min45_fast; break;
    00921     //0+180, no turn:
    00922   default: return -1;
    00923   }
    00924 }
    00925
    00926 void GT2004ParametersSet::mirrorThis(int index)
    00927 {
    00928   int mirror=getIndexOfMirror(index);
    00929   if (mirror>=0)
    00930   {
    00931     GT2004Parameters* src=getParameters(index);
    00932     GT2004Parameters* dest=getParameters(mirror);
    00933     *dest = *src;
    00934     dest->index=mirror;
    00935     dest->requestedMotion.translation.y = -dest->requestedMotion.translation.y;
    00936     dest->requestedMotion.rotation = -dest->requestedMotion.rotation;
    00937     dest->correctedMotion.translation.y = -dest->correctedMotion.translation.y;
    00938     dest->correctedMotion.rotation = -dest->correctedMotion.rotation;
    00939   }
    00940 }
    00941
    00942 double GT2004ParametersSet::getSpeed(const Pose2D& request)
    00943 { return sqrt(sqr(request.translation.abs()/300)+sqr(request.rotation/2.7)); }
    00944
    00945 double GT2004ParametersSet::getRatio(const Pose2D& request)
    00946 { return atan2(request.rotation/2.7,request.translation.abs()/300)*2/pi; }
    00947
    00948 double GT2004ParametersSet::getDirection(const Pose2D& request)
    00949 { return atan2(request.translation.y,request.translation.x); }
    00950
    00951 void GT2004ParametersSet::setSpeed(Pose2D& request, double speed)
    00952 {
    00953   double ratio=getRatio(request);
    00954   double direct=getDirection(request);
    00955   request.rotation=2.7*sin(pi/2*ratio)*speed;
    00956   request.translation.x=300*cos(pi/2*ratio)*cos(direct)*speed;
    00957   request.translation.y=300*cos(pi/2*ratio)*sin(direct)*speed;
    00958   if (fabs(request.rotation)<0.00001) {request.rotation=0;}
    00959   if (fabs(request.translation.x)<0.001) {request.translation.x=0;}
    00960   if (fabs(request.translation.y)<0.001) {request.translation.y=0;}
    00961 }
    00962
    00963 void GT2004ParametersSet::setRatio(Pose2D& request, double ratio)
    00964 {
    00965   double speed=getSpeed(request);
    00966   double direct=getDirection(request);
    00967   request.rotation=2.7*sin(pi/2*ratio)*speed;
    00968   request.translation.x=300*cos(pi/2*ratio)*cos(direct)*speed;
    00969   request.translation.y=300*cos(pi/2*ratio)*sin(direct)*speed;
    00970   if (fabs(request.rotation)<0.00001) {request.rotation=0;}
    00971   if (fabs(request.translation.x)<0.001) {request.translation.x=0;}
    00972   if (fabs(request.translation.y)<0.001) {request.translation.y=0;}
    00973 }
    00974
    00975 void GT2004ParametersSet::setDirection(Pose2D& request, double direct)
    00976 {
    00977   double v=request.translation.abs();
    00978   request.translation.x=cos(direct)*v;
    00979   request.translation.y=sin(direct)*v;
    00980   if (fabs(request.rotation)<0.00001) {request.rotation=0;}
    00981   if (fabs(request.translation.x)<0.001) {request.translation.x=0;}
    00982   if (fabs(request.translation.y)<0.001) {request.translation.y=0;}
    00983 }
    00984
    00985
    00986 bool GT2004ParametersSet::load(char* filename)
    00987 {
    00988   char name[128];
    00989   if (filename==0)
    00990   {
    00991     if (getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210)
    00992     {
    00993       sprintf(name,"udset210.dat");
    00994     }
    00995     else
    00996     {
    00997       sprintf(name,"udset7.dat");
    00998     }
    00999   }
    01000   else
    01001   {
    01002     strcpy(name,filename);
    01003   }
    01004   InBinaryFile file(name);
    01005   if (file.exists())
    01006   {
    01007     file >> *this;
    01008     mergedParameters=&rotationOnly[3];
    01009     return true;
    01010   }
    01011   return false;
    01012 }
    01013
    01014 void GT2004ParametersSet::save(char* filename)
    01015 {
    01016   char name[128];
    01017   if (filename==0)
    01018   {
    01019     if (getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210)
    01020     {
    01021       sprintf(name,"udset210.dat");
    01022     }
    01023     else
    01024     {
    01025       sprintf(name,"udset7.dat");
    01026     }
    01027   }
    01028   else
    01029   {
    01030     strcpy(name,filename);
    01031   }
    01032   OutBinaryFile file(name);
    01033   file << *this;
    01034 }
    01035
    01036 In& operator>>(In& stream,GT2004ParametersSet& walkingParametersSet)
    01037 {
    01038   for (int i=0;i<=6;i++)
    01039   {
    01040     stream >> walkingParametersSet.rotationOnly[i];
    01041   }
    01042   for (int speed=0;speed<=2;speed++)
    01043   {
    01044     for (int rat=0;rat<=4;rat++)
    01045     {
    01046       for (int dir=0;dir<=7;dir++)
    01047       {
    01048         stream >> walkingParametersSet.withWalk[speed][rat][dir];
    01049       }
    01050     }
    01051   }
    01052   return stream;
    01053 }
    01054
    01055 Out& operator<<(Out& stream, const GT2004ParametersSet& walkingParametersSet)
    01056 {
    01057   for (int i=0;i<=6;i++)
    01058   {
    01059     stream << walkingParametersSet.rotationOnly[i];
    01060   }
    01061   for (int speed=0;speed<=2;speed++)
    01062   {
    01063     for (int rat=0;rat<=4;rat++)
    01064     {
    01065       for (int dir=0;dir<=7;dir++)
    01066       {
    01067         stream << walkingParametersSet.withWalk[speed][rat][dir];
    01068       }
    01069     }
    01070   }
    01071   return stream;
    01072 }