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

# Modules/WalkingEngine/InvKinWalkingParameters.h

Go to the documentation of this file.

    00001 /**
    00002 * @file InvKinWalkingParameters.h
    00003 *
    00004 * Definition of class InvKinWalkingParameters
    00005 *
    00006 * @author Max Risler
    00007 */
    00008
    00009 #ifndef __InvKinWalkingParameters_h_
    00010 #define __InvKinWalkingParameters_h_
    00011
    00012 #include "Tools/Evolution/Individual.h"
    00013 #include "Representations/Motion/JointData.h"
    00014 #include "GT2004ParameterSet.h"
    00015 #include <string.h>
    00016
    00017 class GT2004Parameters;
    00018
    00019 /**
    00020 * @class InvKinWalkingParameters
    00021 *
    00022 * Parameters for InvKinWalkingEngine
    00023 *
    00024 * @author Max Risler
    00025 */
    00026 class InvKinWalkingParameters: public Individual
    00027 {
    00028 public:
    00029   char name[100]; ///< name of the parameter set
    00030
    00031   enum FootMode {
    00032     rectangle = 0,
    00033       halfCircle,
    00034       circle,
    00035       rounded,
    00036       optimized,
    00037       freeFormQuad,
    00038       numOfFootModes
    00039   };
    00040
    00041   FootMode footMode; ///< how to move the feet
    00042
    00043   double foreHeight;  ///< fore walking height
    00044   double foreWidth;   ///< fore walking width distance robot center to foot
    00045   double foreCenterX; ///< fore x-axis foot center position
    00046
    00047   double hindHeight;  ///< hind walking height
    00048   double hindWidth;   ///< hind walking width distance robot center to foot
    00049   double hindCenterX; ///< hind x-axis foot center position
    00050
    00051   double foreFootLift;    ///< height of lifted fore foots
    00052   double hindFootLift;    ///< height of lifted hind foots
    00053
    00054   double foreFootTilt;  ///< tangens of tilt angle for fore foot movement
    00055   double hindFootTilt;  ///< tangens of tilt angle for hind foot movement
    00056
    00057   double legSpeedFactorX;  ///< speed factor by which fore legs are faster than hind legs
    00058   double legSpeedFactorY;  ///< speed factor by which fore legs are faster than hind legs
    00059   double legSpeedFactorR;  ///< speed factor by which fore legs are faster than hind legs
    00060
    00061   ///maximum step size
    00062   double maxStepSizeX; //mm/step
    00063   double maxStepSizeY; //mm/step
    00064
    00065   ///maximum change in requested x speed (in mm/s change per step)
    00066   double maxSpeedXChange;
    00067   ///maximum change in requested y speed (in mm/s change per step)
    00068   double maxSpeedYChange;
    00069   ///maximum change in requested rotation speed (in rad/s change per step)
    00070   double maxRotationChange;
    00071
    00072   /**
    00073   * while walking sideways a rotational speed is added to prevent unwanted rotation
    00074   * this factor is multiplied with y speed and added to rotation speed
    00075   */
    00076   double counterRotation;
    00077
    00078   int stepLen;         ///< number of ticks for one complete step
    00079
    00080   double groundPhase[2];   ///<part of leg phase with foot on ground, front/hind legs
    00081   double liftPhase[2];     ///<part of leg phase lifting foot, front/hind legs
    00082   double loweringPhase[2]; ///<part of leg phase lowering foot, front/hind legs
    00083   double legPhase[4]; ///< relative leg phases, time indexes for lifting each leg
    00084
    00085   ///value for shifting body away from lifted legs in x direction (mm when leg fully lifted)
    00086   double bodyShiftX;
    00087   ///value for shifting body away from lifted legs in y direction (mm when leg fully lifted)
    00088   double bodyShiftY;
    00089   ///value by which the shift body motion is delayed (relative part of leg phase)
    00090   double bodyShiftOffset;
    00091
    00092   /**
    00093   * joint angles for joints not used for walking
    00094   * these must be jointDataInvalidValue to enable head motion
    00095   * use these for special walks that use the head eg for grabbing the ball
    00096   */
    00097   long headTilt, headPan, headRoll, mouth;
    00098
    00099   /**
    00100   * foot position coordinates for free form quad mode
    00101   */
    00102   double freeFormQuadPos[2][4][3];
    00103
    00104   /**
    00105   * whether engine can be left anytime or only when all four feet are on ground#
    00106   */
    00107   bool leaveAnytime;
    00108
    00109   //!@name fixed gait parameters
    00110   //!@{
    00111   int groundTime[2];      ///< number of ticks foot is on ground
    00112   int liftTime[2];       ///< number of ticks for lifting foot
    00113   int loweringTime[2];       ///< number of ticks for lowering foot
    00114   int airTime[2];         ///< number of ticks foot is in air
    00115   int stepLift[2];       ///< time index for lifting foot
    00116   int stepAir[2];         ///< time index when foot is fully lifted
    00117   int stepLower[2];       ///< time index for lowering foot
    00118   int legPhaseIndex[4];     ///< leg phases, time indexes for lifting each leg
    00119   int firstStep;       ///< time index for first time all feet are on ground
    00120   //!@}
    00121
    00122   /** neck height in mm while walking */
    00123   double neckHeight;
    00124
    00125   /** body tilt angle */
    00126   double bodyTilt,
    00127          bodyTiltOffset; /**< parameter dependent body tilt offset. */
    00128
    00129   /**
    00130   * maximal step size for rotation steps
    00131   * this is calculated from maximal step sizes
    00132   */
    00133   double maxStepSizeR;
    00134
    00135   /** Factors to convert requested body speed to internal leg speed
    00136   *  these values are read from odometry.cfg */
    00137   class CorrectionValues {
    00138   public:
    00139     /** for walking forward */
    00140     double forward;
    00141     /** for walking backwards */
    00142     double backward;
    00143     /** for walking sideways */
    00144     double sideward;
    00145     /** for turning */
    00146     double turning;
    00147     /** distance from neck joint to center of rotation when turning in x-direction */
    00148     double rotationCenter;
    00149   } correctionValues;
    00150
    00151   /** Constructor */
    00152   InvKinWalkingParameters(
    00153     const char name[] = "",
    00154     int footMode = (int)rectangle,
    00155     double foreHeight = 0,
    00156     double foreWidth = 0,
    00157     double foreCenterX = 0,
    00158     double hindHeight = 0,
    00159     double hindWidth= 0,
    00160     double hindCenterX = 0,
    00161     double foreFootLift = 0,
    00162     double hindFootLift = 0,
    00163     double foreFootTilt = 0,
    00164     double hindFootTilt = 0,
    00165     double legSpeedFactorX = 1.0,
    00166     double legSpeedFactorY = 1.0,
    00167     double legSpeedFactorR = 1.0,
    00168     double maxStepSizeX = 0,
    00169     double maxStepSizeY = 0,
    00170     double maxSpeedXChange = 1000.0,
    00171     double maxSpeedYChange = 1000.0,
    00172     double maxRotationChange = 1000.0,
    00173     double counterRotation = 0,
    00174     int stepLen = 100,
    00175     double groundPhaseF = 0.5,
    00176     double liftPhaseF = 0,
    00177     double loweringPhaseF = 0,
    00178     double groundPhaseH = 0.5,
    00179     double liftPhaseH = 0,
    00180     double loweringPhaseH = 0,
    00181     double legPhase0 = 0,
    00182     double legPhase1 = 0.5,
    00183     double legPhase2 = 0.5,
    00184     double legPhase3 = 0,
    00185     double bodyShiftX = 0,
    00186     double bodyShiftY = 0,
    00187     double bodyShiftOffset = 0,
    00188     double bodyTiltOffset = 0,
    00189     int leaveAnytime = 0,
    00190     long headTilt = jointDataInvalidValue,
    00191     long headPan = jointDataInvalidValue,
    00192     long headRoll = jointDataInvalidValue,
    00193     long mouth = jointDataInvalidValue) :
    00194   footMode((FootMode)footMode),
    00195     foreHeight(foreHeight),
    00196     foreWidth(foreWidth),
    00197     foreCenterX(foreCenterX),
    00198     hindHeight(hindHeight),
    00199     hindWidth(hindWidth),
    00200     hindCenterX(hindCenterX),
    00201     foreFootLift(foreFootLift),
    00202     hindFootLift(hindFootLift),
    00203     foreFootTilt(foreFootTilt),
    00204     hindFootTilt(hindFootTilt),
    00205     legSpeedFactorX(legSpeedFactorX),
    00206     legSpeedFactorY(legSpeedFactorY),
    00207     legSpeedFactorR(legSpeedFactorR),
    00208     maxStepSizeX(maxStepSizeX),
    00209     maxStepSizeY(maxStepSizeY),
    00210     maxSpeedXChange(maxSpeedXChange),
    00211     maxSpeedYChange(maxSpeedYChange),
    00212     maxRotationChange(maxRotationChange),
    00213     counterRotation(counterRotation),
    00214     stepLen(stepLen),
    00215     bodyShiftX(bodyShiftX),
    00216     bodyShiftY(bodyShiftY),
    00217     bodyShiftOffset(bodyShiftOffset),
    00218     bodyTiltOffset(bodyTiltOffset),
    00219     headTilt(headTilt),
    00220     headPan(headPan),
    00221     headRoll(headRoll),
    00222     mouth(mouth),
    00223     leaveAnytime(leaveAnytime!=0)
    00224   {
    00225     strncpy(this->name,name,100);
    00226     legPhase[0]=legPhase0;
    00227     legPhase[1]=legPhase1;
    00228     legPhase[2]=legPhase2;
    00229     legPhase[3]=legPhase3;
    00230     groundPhase[0]=groundPhaseF;
    00231     liftPhase[0]=liftPhaseF;
    00232     loweringPhase[0]=loweringPhaseF;
    00233     groundPhase[1]=groundPhaseH;
    00234     liftPhase[1]=liftPhaseH;
    00235     loweringPhase[1]=loweringPhaseH;
    00236
    00237     init();
    00238     readValues();
    00239   }
    00240
    00241   /** Constructor */
    00242   InvKinWalkingParameters(const GT2004Parameters& gt2004Param);
    00243
    00244   virtual void getDimension(int& dim1, int& dim2)
    00245   {
    00246     dim1=35;
    00247     //dim1=59;
    00248     dim2=1;
    00249   }
    00250
    00251   virtual void getValue(int index1, int index2, double& min, double& max, double& value, ValueType& type)
    00252   {
    00253     switch(index1)
    00254     {
    00255       case 0: value=footMode; min=0; max=4; type=valueInt; break;
    00256       //case 0: value=footMode; min=5; max=5; type=valueInt; break;
    00257       case 1: value=foreHeight; min=30; max=170; type=valueDouble; break;
    00258       case 2: value=foreWidth; min=40; max=120; type=valueDouble; break;
    00259       case 3: value=foreCenterX; min=-20; max=80; type=valueDouble; break;
    00260       case 4: value=hindHeight; min=40; max=190; type=valueDouble; break;
    00261       case 5: value=hindWidth; min=40; max=140; type=valueDouble; break;
    00262       case 6: value=hindCenterX; min=-80; max=20; type=valueDouble; break;
    00263       case 7: value=foreFootLift; min=0; max=40; type=valueDouble; break;
    00264       case 8: value=hindFootLift; min=0; max=40; type=valueDouble; break;
    00265       case 9: value=foreFootTilt; min=-0.4; max=0.4; type=valueDouble; break;
    00266       case 10: value=hindFootTilt; min=-0.4; max=0.44; type=valueDouble; break;
    00267       case 11: value=legSpeedFactorX; min=0.3; max=3; type=valueDouble; break;
    00268       case 12: value=legSpeedFactorY; min=0.3; max=3; type=valueDouble; break;
    00269       case 13: value=legSpeedFactorR; min=0.3; max=3; type=valueDouble; break;
    00270       case 14: value=maxStepSizeX; min=1; max=60; type=valueDouble; break;
    00271       case 15: value=maxStepSizeY; min=1; max=60; type=valueDouble; break;
    00272       case 16: value=maxSpeedXChange; min=10; max=400; type=valueDouble; break;
    00273       case 17: value=maxSpeedYChange; min=10; max=400; type=valueDouble; break;
    00274       case 18: value=maxRotationChange; min=0.1; max=2; type=valueDouble; break;
    00275       case 19: value=counterRotation; min=-0.5; max=0.5; type=valueDouble; break;
    00276       case 20: value=stepLen; min=10; max=200; type=valueInt; break;
    00277       case 21: value=groundPhase[0]; min=0.01; max=0.99; type=valueDouble; break;
    00278       case 22: value=liftPhase[0]; min=0.01; max=0.99; type=valueDouble; break;
    00279       case 23: value=loweringPhase[0]; min=0.01; max=0.99; type=valueDouble; break;
    00280       case 24: value=groundPhase[1]; min=0.01; max=0.99; type=valueDouble; break;
    00281       case 25: value=liftPhase[1]; min=0.01; max=0.99; type=valueDouble; break;
    00282       case 26: value=loweringPhase[1]; min=0.01; max=0.99; type=valueDouble; break;
    00283
    00284       case 28: value=legPhase[0]; min=0; max=0.99; type=valueDouble; break;
    00285       case 29: value=legPhase[1]; min=0; max=0.99; type=valueDouble; break;
    00286       case 30: value=legPhase[2]; min=0; max=0.99; type=valueDouble; break;
    00287       case 31: value=legPhase[3]; min=0; max=0.99; type=valueDouble; break;
    00288       case 32: value=bodyShiftX; min=-30; max=30; type=valueDouble; break;
    00289       case 33: value=bodyShiftY; min=-30; max=30; type=valueDouble; break;
    00290       case 34: value=bodyShiftOffset; min=0; max=0.99; type=valueDouble; break;
    00291 /*
    00292       case 35: value=freeFormQuadPos[0][0][0]; min=-10; max=10; type=valueDouble; break;
    00293       case 36: value=freeFormQuadPos[0][0][1]; min=-10; max=10; type=valueDouble; break;
    00294       case 37: value=freeFormQuadPos[0][0][2]; min=-10; max=10; type=valueDouble; break;
    00295       case 38: value=freeFormQuadPos[0][1][0]; min=-10; max=10; type=valueDouble; break;
    00296       case 39: value=freeFormQuadPos[0][1][1]; min=-10; max=10; type=valueDouble; break;
    00297       case 40: value=freeFormQuadPos[0][1][2]; min=-10; max=10; type=valueDouble; break;
    00298       case 41: value=freeFormQuadPos[0][2][0]; min=-10; max=10; type=valueDouble; break;
    00299       case 42: value=freeFormQuadPos[0][2][1]; min=-10; max=10; type=valueDouble; break;
    00300       case 43: value=freeFormQuadPos[0][2][2]; min=-10; max=10; type=valueDouble; break;
    00301       case 44: value=freeFormQuadPos[0][3][0]; min=-10; max=10; type=valueDouble; break;
    00302       case 45: value=freeFormQuadPos[0][3][1]; min=-10; max=10; type=valueDouble; break;
    00303       case 46: value=freeFormQuadPos[0][3][2]; min=-10; max=10; type=valueDouble; break;
    00304       case 47: value=freeFormQuadPos[1][0][0]; min=-10; max=10; type=valueDouble; break;
    00305       case 48: value=freeFormQuadPos[1][0][1]; min=-10; max=10; type=valueDouble; break;
    00306       case 49: value=freeFormQuadPos[1][0][2]; min=-10; max=10; type=valueDouble; break;
    00307       case 50: value=freeFormQuadPos[1][1][0]; min=-10; max=10; type=valueDouble; break;
    00308       case 51: value=freeFormQuadPos[1][1][1]; min=-10; max=10; type=valueDouble; break;
    00309       case 52: value=freeFormQuadPos[1][1][2]; min=-10; max=10; type=valueDouble; break;
    00310       case 53: value=freeFormQuadPos[1][2][0]; min=-10; max=10; type=valueDouble; break;
    00311       case 54: value=freeFormQuadPos[1][2][1]; min=-10; max=10; type=valueDouble; break;
    00312       case 55: value=freeFormQuadPos[1][2][2]; min=-10; max=10; type=valueDouble; break;
    00313       case 56: value=freeFormQuadPos[1][3][0]; min=-10; max=10; type=valueDouble; break;
    00314       case 57: value=freeFormQuadPos[1][3][1]; min=-10; max=10; type=valueDouble; break;
    00315       case 58: value=freeFormQuadPos[1][3][2]; min=-10; max=10; type=valueDouble; break;
    00316 */
    00317     }
    00318   }
    00319
    00320   virtual void setValue(int index1, int index2, double value)
    00321   {
    00322     switch(index1)
    00323     {
    00324       case 0: footMode=(FootMode)((int)value); break;
    00325       case 1: foreHeight=value; break;
    00326       case 2: foreWidth=value; break;
    00327       case 3: foreCenterX=value; break;
    00328       case 4: hindHeight=value; break;
    00329       case 5: hindWidth=value; break;
    00330       case 6: hindCenterX=value; break;
    00331       case 7: foreFootLift=value; break;
    00332       case 8: hindFootLift=value; break;
    00333       case 9: foreFootTilt=value; break;
    00334       case 10: hindFootTilt=value; break;
    00335       case 11: legSpeedFactorX=value; break;
    00336       case 12: legSpeedFactorY=value; break;
    00337       case 13: legSpeedFactorR=value; break;
    00338       case 14: maxStepSizeX=value; break;
    00339       case 15: maxStepSizeY=value; break;
    00340       case 16: maxSpeedXChange=value; break;
    00341       case 17: maxSpeedYChange=value; break;
    00342       case 18: maxRotationChange=value; break;
    00343       case 19: counterRotation=value; break;
    00344       case 20: stepLen=(int)value; break;
    00345       case 21: groundPhase[0]=value; break;
    00346       case 22: liftPhase[0]=value; break;
    00347       case 23: loweringPhase[0]=value; break;
    00348       case 24: groundPhase[1]=value; break;
    00349       case 25: liftPhase[1]=value; break;
    00350       case 26: loweringPhase[1]=value; break;
    00351
    00352       case 28: legPhase[0]=value; break;
    00353       case 29: legPhase[1]=value; break;
    00354       case 30: legPhase[2]=value; break;
    00355       case 31: legPhase[3]=value; break;
    00356       case 32: bodyShiftX=value; break;
    00357       case 33: bodyShiftY=value; break;
    00358       case 34: bodyShiftOffset=value; break;
    00359 /*
    00360       case 35: freeFormQuadPos[0][0][0]=value; break;
    00361       case 36: freeFormQuadPos[0][0][1]=value; break;
    00362       case 37: freeFormQuadPos[0][0][2]=value; break;
    00363       case 38: freeFormQuadPos[0][1][0]=value; break;
    00364       case 39: freeFormQuadPos[0][1][1]=value; break;
    00365       case 40: freeFormQuadPos[0][1][2]=value; break;
    00366       case 41: freeFormQuadPos[0][2][0]=value; break;
    00367       case 42: freeFormQuadPos[0][2][1]=value; break;
    00368       case 43: freeFormQuadPos[0][2][2]=value; break;
    00369       case 44: freeFormQuadPos[0][3][0]=value; break;
    00370       case 45: freeFormQuadPos[0][3][1]=value; break;
    00371       case 46: freeFormQuadPos[0][3][2]=value; break;
    00372       case 47: freeFormQuadPos[1][0][0]=value; break;
    00373       case 48: freeFormQuadPos[1][0][1]=value; break;
    00374       case 49: freeFormQuadPos[1][0][2]=value; break;
    00375       case 50: freeFormQuadPos[1][1][0]=value; break;
    00376       case 51: freeFormQuadPos[1][1][1]=value; break;
    00377       case 52: freeFormQuadPos[1][1][2]=value; break;
    00378       case 53: freeFormQuadPos[1][2][0]=value; break;
    00379       case 54: freeFormQuadPos[1][2][1]=value; break;
    00380       case 55: freeFormQuadPos[1][2][2]=value; break;
    00381       case 56: freeFormQuadPos[1][3][0]=value; break;
    00382       case 57: freeFormQuadPos[1][3][1]=value; break;
    00383       case 58: freeFormQuadPos[1][3][2]=value; break;
    00384 */
    00385     }
    00386   }
    00387
    00388
    00389 #define interpolateInvalid(v1,v2,factor1) \
    00390   (((v1==jointDataInvalidValue)||(v2==jointDataInvalidValue))? \
    00391   ((factor1<0.5)?v2:v1):(factor1*v1+(1-factor1)*v2));
    00392
    00393   inline void interpolate(InvKinWalkingParameters& p1, InvKinWalkingParameters& p2, double factor1)
    00394   {
    00395     footMode=(factor1<0.5)?p2.footMode:p1.footMode;
    00396
    00397     foreHeight=factor1*p1.foreHeight+(1-factor1)*p2.foreHeight;
    00398     foreWidth=factor1*p1.foreWidth+(1-factor1)*p2.foreWidth;
    00399     foreCenterX=factor1*p1.foreCenterX+(1-factor1)*p2.foreCenterX;
    00400
    00401     hindHeight=factor1*p1.hindHeight+(1-factor1)*p2.hindHeight;
    00402     hindWidth=factor1*p1.hindWidth+(1-factor1)*p2.hindWidth;
    00403     hindCenterX=factor1*p1.hindCenterX+(1-factor1)*p2.hindCenterX;
    00404
    00405     foreFootLift=factor1*p1.foreFootLift+(1-factor1)*p2.foreFootLift;
    00406     hindFootLift=factor1*p1.hindFootLift+(1-factor1)*p2.hindFootLift;
    00407     foreFootTilt=factor1*p1.foreFootTilt+(1-factor1)*p2.foreFootTilt;
    00408     hindFootTilt=factor1*p1.hindFootTilt+(1-factor1)*p2.hindFootTilt;
    00409
    00410     legSpeedFactorX=factor1*p1.legSpeedFactorX+(1-factor1)*p2.legSpeedFactorX;
    00411     legSpeedFactorY=factor1*p1.legSpeedFactorY+(1-factor1)*p2.legSpeedFactorY;
    00412     legSpeedFactorR=factor1*p1.legSpeedFactorR+(1-factor1)*p2.legSpeedFactorR;
    00413
    00414     maxStepSizeX=factor1*p1.maxStepSizeX+(1-factor1)*p2.maxStepSizeX;
    00415     maxStepSizeY=factor1*p1.maxStepSizeY+(1-factor1)*p2.maxStepSizeY;
    00416     maxSpeedXChange=factor1*p1.maxSpeedXChange+(1-factor1)*p2.maxSpeedXChange;
    00417     maxSpeedYChange=factor1*p1.maxSpeedYChange+(1-factor1)*p2.maxSpeedYChange;
    00418     maxRotationChange=factor1*p1.maxRotationChange+(1-factor1)*p2.maxRotationChange;
    00419
    00420     counterRotation=factor1*p1.counterRotation+(1-factor1)*p2.counterRotation;
    00421     stepLen=(int)(factor1*p1.stepLen+(1-factor1)*p2.stepLen);
    00422     groundPhase[0]=factor1*p1.groundPhase[0]+(1-factor1)*p2.groundPhase[0];
    00423     liftPhase[0]=factor1*p1.liftPhase[0]+(1-factor1)*p2.liftPhase[0];
    00424     loweringPhase[0]=factor1*p1.loweringPhase[0]+(1-factor1)*p2.loweringPhase[0];
    00425     groundPhase[1]=factor1*p1.groundPhase[1]+(1-factor1)*p2.groundPhase[1];
    00426     liftPhase[1]=factor1*p1.liftPhase[1]+(1-factor1)*p2.liftPhase[1];
    00427     loweringPhase[1]=factor1*p1.loweringPhase[1]+(1-factor1)*p2.loweringPhase[1];
    00428
    00429     bodyShiftX=factor1*p1.bodyShiftX+(1-factor1)*p2.bodyShiftX;
    00430     bodyShiftY=factor1*p1.bodyShiftY+(1-factor1)*p2.bodyShiftY;
    00431     bodyShiftOffset=factor1*p1.bodyShiftOffset+(1-factor1)*p2.bodyShiftOffset;
    00432     bodyTiltOffset=factor1*p1.bodyTiltOffset+(1-factor1)*p2.bodyTiltOffset;
    00433
    00434     headTilt=(long)interpolateInvalid(p1.headTilt,p2.headTilt,factor1);
    00435     headPan=(long)interpolateInvalid(p1.headPan,p2.headPan,factor1);
    00436     headRoll=(long)interpolateInvalid(p1.headRoll,p2.headRoll,factor1);
    00437     mouth=(long)interpolateInvalid(p1.mouth,p2.mouth,factor1);
    00438
    00439     legPhase[0]=factor1*p1.legPhase[0]+(1-factor1)*p2.legPhase[0];
    00440     legPhase[1]=factor1*p1.legPhase[1]+(1-factor1)*p2.legPhase[1];
    00441     legPhase[2]=factor1*p1.legPhase[2]+(1-factor1)*p2.legPhase[2];
    00442     legPhase[3]=factor1*p1.legPhase[3]+(1-factor1)*p2.legPhase[3];
    00443
    00444     correctionValues.forward=factor1*p1.correctionValues.forward+(1-factor1)*p2.correctionValues.forward;
    00445     correctionValues.sideward=factor1*p1.correctionValues.sideward+(1-factor1)*p2.correctionValues.sideward;
    00446     correctionValues.turning=factor1*p1.correctionValues.turning+(1-factor1)*p2.correctionValues.turning;
    00447     correctionValues.rotationCenter=factor1*p1.correctionValues.rotationCenter+(1-factor1)*p2.correctionValues.rotationCenter;
    00448     neckHeight=factor1*p1.neckHeight+(1-factor1)*p2.neckHeight;
    00449
    00450     for (int i=0; i<2; i++)
    00451       for (int j=0; j<4; j++)
    00452         for (int k=0; k<3; k++)
    00453           freeFormQuadPos[i][j][k]=(factor1<0.5)?p2.freeFormQuadPos[i][j][k]:p1.freeFormQuadPos[i][j][k];
    00454
    00455     if (factor1 == 1.0)
    00456       leaveAnytime = p1.leaveAnytime;
    00457     else if (factor1 == 0.0)
    00458       leaveAnytime = p2.leaveAnytime;
    00459     else
    00460       leaveAnytime = p2.leaveAnytime || p1.leaveAnytime;
    00461   }
    00462
    00463   /** copies the walking parameters without correctionValues */
    00464   void copyValuesFrom(const InvKinWalkingParameters & paramsToCopy);
    00465
    00466   /** initialise fixed parameters */
    00467   void init();
    00468
    00469   /** reads the correction values, neck height from odometry.cfg */
    00470   void readValues();
    00471
    00472   /** returns true when all feet are on ground */
    00473   bool allFeetOnGround(int currentStep);
    00474 };
    00475
    00476 /**
    00477 * Streaming operator that reads InvKinWalkingParameters from a stream.
    00478 * @param stream The stream from which is read.
    00479 * @param walkingParameters The InvKinWalkingParameters object.
    00480 * @return The stream.
    00481 */
    00482 In& operator>>(In& stream,InvKinWalkingParameters& walkingParameters);
    00483
    00484 /**
    00485 * Streaming operator that writes InvKinWalkingParameters to a stream.
    00486 * @param stream The stream to write on.
    00487 * @param walkingParameters The InvKinWalkingParameters object.
    00488 * @return The stream.
    00489 */
    00490 Out& operator<<(Out& stream, const InvKinWalkingParameters& walkingParameters);
    00491
    00492 #endif// __InvKinWalkingParameters_h_