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

# Modules/MotionControl/GT2005MotionControl.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005MotionControl.cpp
    00003 *
    00004 * Implementation of class GT2005MotionControl.
    00005 *
    00006 * @author Max Risler
    00007 */
    00008
    00009 #include "GT2005MotionControl.h"
    00010
    00011 #include "Modules/WalkingEngine/WalkingEngineSelector.h"
    00012 #include "Modules/WalkingEngine/InvKinWalkingEngine.h"
    00013 #include "Modules/WalkingEngine/InvKinWalkingParameterSets.h"
    00014 #include "Modules/SpecialActions/GT2003MotionNetSpecialActions/GT2003MotionNetSpecialActions.h"
    00015 #include "Platform/GTAssert.h"
    00016 #include "Platform/SystemCall.h"
    00017 #include "Tools/Actorics/Kinematics.h"
    00018 #include "Tools/Debugging/Debugging.h"
    00019 #include "Tools/Debugging/GenericDebugData.h"
    00020 #include "Tools/RobotConfiguration.h"
    00021
    00022 GT2005MotionControl::GT2005MotionControl(ModuleHandler& moduleHandler,
    00023                                            const MotionControlInterfaces& interfaces)
    00024                                            :MotionControl(interfaces),
    00025                                            moduleHandler(moduleHandler),
    00026                                            changeOfMotionControlStatePossible(true),
    00027                                            lastMotionType(MotionRequest::playDead),
    00028                                            motionControlState(GT2005MotionControl::playDead)
    00029 {
    00030   // create an uninitialised motion request to set startup motion -- something like getup (see MotionRequest-Constructor)
    00031   MotionRequest defaultRequest;
    00032   // currentMotionType = lastMotionType = defaultRequest.motionType;
    00033   lastHeadTilt = lastHeadPan = lastHeadRoll = 0;
    00034
    00035   WalkingEngineInterfaces walkingEngineInterfaces
    00036     (sensorDataBuffer, invKinWalkingParameters, gt2004Parameters, walkParameterTimeStamp, receivedNewSensorData, lastMotionType, pidData, odometryData, motionInfo, gt2005Parameters, gt2005DebugData); //****for GT2005WalkingEngine****
    00037
    00038   // create the InvKinWalking engine as only one instance is created
    00039   // and used by different walk type with different parameter sets
    00040   pInvKinWalkingEngine = new InvKinWalkingEngine(walkingEngineInterfaces);
    00041
    00042   WalkingEngineSelector* tmp = new WalkingEngineSelector(moduleHandler, SolutionRequest::walkingEngineNormal,
    00043     walkingEngineInterfaces, pInvKinWalkingEngine);
    00044   pWalkingEngine[WalkRequest::normal] = tmp;
    00045
    00046   pWalkingEngine[WalkRequest::upsideDown] =
    00047     new ParamInvKinWalkingEngine(new UpsideDownWalkingParameters, pInvKinWalkingEngine);
    00048
    00049   pWalkingEngine[WalkRequest::turnWithBall] =
    00050     new ParamInvKinWalkingEngine(new TurnWithBallWalkingParameters, pInvKinWalkingEngine);
    00051
    00052   pWalkingEngine[WalkRequest::turnKick] =
    00053     new ParamInvKinWalkingEngine(new TurnWithBallWalkingParameters, pInvKinWalkingEngine);
    00054
    00055   pWalkingEngine[WalkRequest::dash] = tmp;
    00056   pWalkingEngine[WalkRequest::debug] =
    00057     new ParamInvKinWalkingEngine(new ERS7EvolveWalkingParameters, pInvKinWalkingEngine);
    00058
    00059   pWalkingEngine[WalkRequest::boost] = tmp;
    00060
    00061   pWalkingEngine[WalkRequest::walkWithBall] = tmp;
    00062
    00063   pWalkingEngine[WalkRequest::gt2004mode] =
    00064     new GT2004WalkingEngine(walkingEngineInterfaces);
    00065
    00066   SpecialActionsInterfaces specialActionsInterfaces(motionRequest,lastMotionType,
    00067     pidData, odometryData, motionInfo);
    00068   pSpecialActions = new GT2003MotionNetSpecialActions(specialActionsInterfaces);
    00069
    00070   GetupEngineInterfaces getupEngineInterfaces(lastMotionType, sensorDataBuffer, motionRequest, bodyPosture, pidData, odometryData,
    00071     motionInfo);
    00072   pGetupEngine = new GetupEngineSelector(moduleHandler,getupEngineInterfaces);
    00073
    00074   wakingUp = false;
    00075   // lastSpecialActionWasPlayDead = false;
    00076 }
    00077
    00078 GT2005MotionControl::~GT2005MotionControl()
    00079 {
    00080   delete pWalkingEngine[WalkRequest::normal];
    00081   delete pWalkingEngine[WalkRequest::gt2004mode];
    00082   delete pWalkingEngine[WalkRequest::upsideDown];
    00083   delete pWalkingEngine[WalkRequest::turnWithBall];
    00084   delete pWalkingEngine[WalkRequest::turnKick];
    00085   delete pWalkingEngine[WalkRequest::debug];
    00086   delete pSpecialActions;
    00087   delete pGetupEngine;
    00088   delete pInvKinWalkingEngine;
    00089
    00090   moduleHandler.setModuleSelector(SolutionRequest::walkingEngineNormal,0);
    00091   moduleHandler.setModuleSelector(SolutionRequest::getupEngine,0);
    00092 }
    00093
    00094 void GT2005MotionControl::determineMotionControlState()
    00095 {
    00096   switch(motionControlState)
    00097   {
    00098   case GT2005MotionControl::playDead:
    00099     {
    00100       if(motionRequest.motionType != MotionRequest::playDead)
    00101       {
    00102         motionControlState = GT2005MotionControl::wakeUp;
    00103       }
    00104       return;
    00105     }
    00106   case GT2005MotionControl::getup:
    00107     if(changeOfMotionControlStatePossible)
    00108     {
    00109         setMotionControlState();
    00110
    00111     }
    00112     return;
    00113   case GT2005MotionControl::wakeUp:
    00114     if(changeOfMotionControlStatePossible)
    00115     {
    00116       setMotionControlState();
    00117
    00118     }
    00119     return;
    00120   case GT2005MotionControl::stand:
    00121     if(changeOfMotionControlStatePossible)
    00122     {
    00123       setMotionControlState();
    00124     }
    00125     return;
    00126   case GT2005MotionControl::specialAction:
    00127     if(changeOfMotionControlStatePossible)
    00128     {
    00129       setMotionControlState();
    00130     }
    00131     return;
    00132   case GT2005MotionControl::walk:
    00133     if(
    00134       changeOfMotionControlStatePossible &&
    00135       ((motionRequest.motionType != MotionRequest::specialAction ||
    00136         pSpecialActions->specialActionIsExecutableInWalkingCycle(motionRequest.specialActionRequest.specialActionType, positionInWalkCycle))
    00137       ))
    00138     {
    00139       setMotionControlState();
    00140     }
    00141     return;
    00142   }
    00143 }
    00144
    00145 void GT2005MotionControl::setMotionControlState()
    00146 {
    00147   switch(motionRequest.motionType)
    00148   {
    00149   case MotionRequest::playDead:
    00150     motionControlState = GT2005MotionControl::playDead;
    00151     return;
    00152   case MotionRequest::getup:
    00153     motionControlState = GT2005MotionControl::getup;
    00154     return;
    00155   case MotionRequest::stand:
    00156     motionControlState = GT2005MotionControl::stand;
    00157     return;
    00158   case MotionRequest::specialAction:
    00159     motionControlState = GT2005MotionControl::specialAction;
    00160     return;
    00161   case MotionRequest::walk:
    00162     motionControlState = GT2005MotionControl::walk;
    00163     return;
    00164   }
    00165 }
    00166 void GT2005MotionControl::execute()
    00167 {
    00168   for (int i = 0; i < jointDataBufferNumOfFrames; i++)
    00169   {
    00170
    00171     JointData &currentFrame = jointDataBuffer.frame[i];
    00172
    00173     // remember some variables
    00174     if(motionRequest.motionType == MotionRequest::walk)
    00175       latestWalkRequest = motionRequest.walkRequest;
    00176     else if(motionRequest.motionType == MotionRequest::specialAction)
    00177       latestSpecialActionRequest = motionRequest.specialActionRequest;
    00178
    00179
    00180     // determine current state for Motion Control
    00181     determineMotionControlState();
    00182
    00183     /* state machine for current action*/
    00184     switch(motionControlState)
    00185     {
    00186     case GT2005MotionControl::playDead:
    00187       {
    00188         changeOfMotionControlStatePossible = true;
    00189         for(int i = 0; i<JointData::numOfJoint; ++i)
    00190         {
    00191           pidData.p[i]=0;pidData.i[i]=0;pidData.d[i]=0;
    00192         }
    00193         motionInfo.executedMotionRequest.motionType = MotionRequest::playDead;
    00194         lastMotionType = MotionRequest::playDead;
    00195         break;
    00196       }
    00197     case GT2005MotionControl::wakeUp:
    00198       {
    00199         changeOfMotionControlStatePossible
    00200           = !wakeUpEngine.execute(frameNumber, sensorDataBuffer.frame[0], currentFrame, pidData);
    00201         lastMotionType = MotionRequest::playDead;
    00202         break;
    00203       }
    00204       case GT2005MotionControl::getup:
    00205         {
    00206           changeOfMotionControlStatePossible = !pGetupEngine->executeParameterized(currentFrame);
    00207           lastMotionType = MotionRequest::getup;
    00208           break;
    00209         }
    00210     case GT2005MotionControl::stand:
    00211       {
    00212         WalkRequest standRequest;
    00213         standRequest.walkType = WalkRequest::normal;
    00214         standRequest.walkParams = Pose2D(0,0,0);
    00215         changeOfMotionControlStatePossible
    00216           = !pWalkingEngine[WalkRequest::normal]->executeParameterized(currentFrame,standRequest,0);
    00217         motionInfo.executedMotionRequest.motionType = MotionRequest::stand;
    00218         positionInWalkCycle = motionInfo.positionInWalkCycle;
    00219         lastMotionType = MotionRequest::stand;
    00220         break;
    00221       }
    00222
    00223     case GT2005MotionControl::specialAction:
    00224       changeOfMotionControlStatePossible
    00225         = !pSpecialActions->executeParameterized(latestSpecialActionRequest, currentFrame);
    00226       lastMotionType = MotionRequest::specialAction;
    00227       break;
    00228
    00229     case GT2005MotionControl::walk:
    00230       {
    00231         changeOfMotionControlStatePossible
    00232           = !pWalkingEngine[latestWalkRequest.walkType]->executeParameterized(currentFrame, latestWalkRequest, positionInWalkCycle);
    00233         positionInWalkCycle = motionInfo.positionInWalkCycle;
    00234         lastMotionType = MotionRequest::walk;
    00235         break;
    00236       }
    00237     }
    00238
    00239     if(motionControlState != GT2005MotionControl::walk){
    00240       positionInWalkCycle = 0;
    00241     }
    00242
    00243     /* end of state machine*/
    00244
    00245     headIsBlockedBySpecialActionOrWalk =
    00246       (
    00247       currentFrame.data[JointData::neckTilt] != jointDataInvalidValue &&
    00248       currentFrame.data[JointData::headPan] != jointDataInvalidValue &&
    00249       currentFrame.data[JointData::headTilt] != jointDataInvalidValue
    00250       );
    00251
    00252     // execute HeadMotionRequest (smoothed)
    00253     const long maxdiff=50000; //(~2pi/s)
    00254     long diff;
    00255     if (currentFrame.data[JointData::neckTilt] == jointDataInvalidValue)
    00256     {
    00257       diff=headMotionRequest.tilt-lastHeadTilt;
    00258       if (diff<-maxdiff)
    00259       {
    00260         currentFrame.data[JointData::neckTilt] = lastHeadTilt-maxdiff;
    00261       }
    00262       else if (diff<maxdiff)
    00263       {
    00264         currentFrame.data[JointData::neckTilt] = headMotionRequest.tilt;
    00265       }
    00266       else
    00267       {
    00268         currentFrame.data[JointData::neckTilt] = lastHeadTilt+maxdiff;
    00269       }
    00270     }
    00271     if (currentFrame.data[JointData::headPan] == jointDataInvalidValue)
    00272     {
    00273       diff=headMotionRequest.pan-lastHeadPan;
    00274       if (diff<-maxdiff)
    00275       {
    00276         currentFrame.data[JointData::headPan] = lastHeadPan-maxdiff;
    00277       }
    00278       else if (diff<maxdiff)
    00279       {
    00280         currentFrame.data[JointData::headPan] = headMotionRequest.pan;
    00281       }
    00282       else
    00283       {
    00284         currentFrame.data[JointData::headPan] = lastHeadPan+maxdiff;
    00285       }
    00286     }
    00287     if (currentFrame.data[JointData::headTilt] == jointDataInvalidValue)
    00288     {
    00289       diff=headMotionRequest.roll-lastHeadRoll;
    00290       if (diff<-maxdiff)
    00291       {
    00292         currentFrame.data[JointData::headTilt] = lastHeadRoll-maxdiff;
    00293       }
    00294       else if (diff<maxdiff)
    00295       {
    00296         currentFrame.data[JointData::headTilt] = headMotionRequest.roll;
    00297       }
    00298       else
    00299       {
    00300         currentFrame.data[JointData::headTilt] = lastHeadRoll+maxdiff;
    00301       }
    00302     }
    00303     if (currentFrame.data[JointData::mouth] == jointDataInvalidValue)
    00304     {
    00305       currentFrame.data[JointData::mouth] = headMotionRequest.mouth;
    00306     }
    00307
    00308     lastHeadTilt=currentFrame.data[JointData::neckTilt];
    00309     lastHeadPan =currentFrame.data[JointData::headPan];
    00310     lastHeadRoll=currentFrame.data[JointData::headTilt];
    00311
    00312     // the obgligatory GT tail wag
    00313     /** @todo:*/
    00314     DEBUG_RESPONSE("LED, tail, mouth:tail shows frameloss",
    00315       wagTail(motionRequest, sensorDataBuffer.lastFrame(), currentFrame);
    00316     );
    00317     /**/
    00318
    00319     // whether or not stabilize does something is determined in the method.
    00320     // it is called because of averaging that needs to be done continously!!
    00321     stabilize(lastMotionType, motionRequest, currentFrame, odometryData, sensorDataBuffer);
    00322   }
    00323 }
    00324
    00325 void GT2005MotionControl::wagTail(
    00326                                    const MotionRequest& motionRequest,
    00327                                    const SensorData& sensorData,
    00328                                    JointData& jointData
    00329                                    )
    00330 {
    00331   if ((jointData.data[JointData::tailPan] != jointDataInvalidValue) ||
    00332     (jointData.data[JointData::tailTilt] != jointDataInvalidValue))
    00333     return;
    00334
    00335   switch (motionRequest.tailRequest.tailRequestID)
    00336   {
    00337   case TailRequest::wagHorizontal:
    00338     {
    00339       int framesPerPeriod = 200;
    00340       tailCount %= framesPerPeriod;
    00341       tailCount++;
    00342       jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanP * sin(tailCount * 2 * pi / framesPerPeriod));
    00343       jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltN);
    00344     }
    00345     break;
    00346   case TailRequest::wagLeftRightHorizontal:
    00347     {
    00348       int framesPerPeriod = 125;
    00349       tailCount %= framesPerPeriod;
    00350       tailCount++;
    00351       jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanP * sin(tailCount * 2 * pi / framesPerPeriod));
    00352       jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltN);
    00353     }
    00354     break;
    00355   case TailRequest::wagLeftRightVertical:
    00356     {
    00357       int framesPerPeriod = 125;
    00358       tailCount %= framesPerPeriod;
    00359       tailCount++;
    00360       jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanP * sin(tailCount * 2 * pi / framesPerPeriod));
    00361       jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltN);
    00362     }
    00363     break;
    00364   case TailRequest::wagHorizontalFast:
    00365     {
    00366       int framesPerPeriod = 30;
    00367       tailCount %= framesPerPeriod;
    00368       tailCount++;
    00369       jointData.data[JointData::tailPan] = toMicroRad((jointLimitTailPanP/2) * sgn(sin(tailCount * 2 * pi / framesPerPeriod)));
    00370       jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltN);
    00371     }
    00372     break;
    00373   case TailRequest::wagVertical:
    00374     {
    00375       int framesPerPeriod = 125;
    00376       tailCount %= framesPerPeriod;
    00377       tailCount++;
    00378       jointData.data[JointData::tailTilt] =
    00379         toMicroRad(
    00380         (jointLimitTailTiltP + jointLimitTailTiltN) / 2 +
    00381         (jointLimitTailTiltP - jointLimitTailTiltN) / 2 * sin(tailCount * 2 * pi / framesPerPeriod)
    00382         );
    00383       jointData.data[JointData::tailPan] = 0;
    00384     }
    00385     break;
    00386   case TailRequest::wagUpDownLeft:
    00387     {
    00388       int framesPerPeriod = 125;
    00389       tailCount %= framesPerPeriod;
    00390       tailCount++;
    00391       jointData.data[JointData::tailTilt] =
    00392         toMicroRad(
    00393         (jointLimitTailTiltP + jointLimitTailTiltN) / 2 +
    00394         (jointLimitTailTiltP - jointLimitTailTiltN) / 2 * sin(tailCount * 2 * pi / framesPerPeriod)
    00395         );
    00396       jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanN);
    00397     }
    00398     break;
    00399   case TailRequest::wagUpDownRight:
    00400     {
    00401       int framesPerPeriod = 125;
    00402       tailCount %= framesPerPeriod;
    00403       tailCount++;
    00404       jointData.data[JointData::tailTilt] =
    00405         toMicroRad(
    00406         (jointLimitTailTiltP + jointLimitTailTiltN) / 2 +
    00407         (jointLimitTailTiltP - jointLimitTailTiltN) / 2 * sin(tailCount * 2 * pi / framesPerPeriod)
    00408         );
    00409       jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanP);
    00410     }
    00411     break;
    00412   case TailRequest::wagVerticalFast:
    00413     {
    00414       tailCount &= 7;
    00415       int j=tailCount++;
    00416       if (j>3) j = 8-j;
    00417       jointData.data[JointData::tailTilt] = 160000*(j - 2);
    00418       jointData.data[JointData::tailPan] = 0;
    00419     }
    00420     break;
    00421   case TailRequest::stayAsForced:
    00422     jointData.data[JointData::tailTilt] = sensorData.data[SensorData::tailTilt];
    00423     jointData.data[JointData::tailPan] = sensorData.data[SensorData::tailPan];
    00424     break;
    00425   case TailRequest::stayAsForcedPan:
    00426     jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltP);
    00427     jointData.data[JointData::tailPan] = sensorData.data[SensorData::tailPan];
    00428     break;
    00429   case TailRequest::stayAsForcedTilt:
    00430     jointData.data[JointData::tailTilt] = sensorData.data[SensorData::tailTilt];
    00431     jointData.data[JointData::tailPan] = 0;
    00432     break;
    00433   case TailRequest::tailFollowsHead:
    00434     jointData.data[JointData::tailTilt] = sensorData.data[SensorData::neckTilt];
    00435     jointData.data[JointData::tailPan] = -sensorData.data[SensorData::headPan];
    00436     break;
    00437   case TailRequest::tailParallelToGround:
    00438     jointData.data[JointData::tailTilt] = 0;
    00439     jointData.data[JointData::tailPan] = 0;
    00440     break;
    00441   case TailRequest::noTailWag:
    00442     jointData.data[JointData::tailTilt] = 0;
    00443     jointData.data[JointData::tailPan] = 0;
    00444     break;
    00445   case TailRequest::twoPositionSwitchVertical:
    00446     jointData.data[JointData::tailPan] = 0;
    00447     if (sensorData.data[SensorData::tailTilt] > 0)
    00448       jointData.data[JointData::tailTilt] = 380000;
    00449     else
    00450       jointData.data[JointData::tailTilt] = -380000;
    00451     break;
    00452   case TailRequest::twoPositionSwitchHorizontal:
    00453     jointData.data[JointData::tailTilt] = 0;
    00454     if (sensorData.data[SensorData::tailPan] > 0)
    00455       jointData.data[JointData::tailPan] = 380000;
    00456     else
    00457       jointData.data[JointData::tailPan] = -380000;
    00458     break;
    00459   case TailRequest::threePositionSwitchVertical:
    00460     jointData.data[JointData::tailPan] = 0;
    00461     if (sensorData.data[SensorData::tailTilt] > 130000)
    00462       jointData.data[JointData::tailTilt] = 380000;
    00463     else if(sensorData.data[SensorData::tailTilt] > -130000)
    00464       jointData.data[JointData::tailTilt] = 0;
    00465     else
    00466       jointData.data[JointData::tailTilt] = -380000;
    00467     break;
    00468   case TailRequest::threePositionSwitchHorizontal:
    00469     jointData.data[JointData::tailTilt] = 0;
    00470     if (sensorData.data[SensorData::tailPan] > 130000)
    00471       jointData.data[JointData::tailPan] = 380000;
    00472     else if(sensorData.data[SensorData::tailPan] > -130000)
    00473       jointData.data[JointData::tailPan] = 0;
    00474     else
    00475       jointData.data[JointData::tailPan] = -380000;
    00476     break;
    00477   case TailRequest::fourPositionSwitchUpDownLeftRight:
    00478     if(sensorData.data[SensorData::tailPan] > sensorData.data[SensorData::tailTilt])
    00479     {
    00480       if(sensorData.data[SensorData::tailPan] > -sensorData.data[SensorData::tailTilt])
    00481       {
    00482         //right
    00483         jointData.data[JointData::tailPan] = 380000;
    00484         jointData.data[JointData::tailTilt] = 0;
    00485       }
    00486       else
    00487       {
    00488         //bottom
    00489         jointData.data[JointData::tailPan] = 0;
    00490         jointData.data[JointData::tailTilt] = -380000;
    00491       }
    00492     }
    00493     else
    00494     {
    00495       if(sensorData.data[SensorData::tailPan] > -sensorData.data[SensorData::tailTilt])
    00496       {
    00497         //top
    00498         jointData.data[JointData::tailPan] = 0;
    00499         jointData.data[JointData::tailTilt] = 380000;
    00500       }
    00501       else
    00502       {
    00503         //left
    00504         jointData.data[JointData::tailPan] = -380000;
    00505         jointData.data[JointData::tailTilt] = 0;
    00506       }
    00507     }
    00508     break;
    00509   case TailRequest::fourPositionSwitchCorners:
    00510     if (sensorData.data[SensorData::tailTilt] > 0)
    00511       jointData.data[JointData::tailTilt] = 380000;
    00512     else
    00513       jointData.data[JointData::tailTilt] = -380000;
    00514
    00515     if (sensorData.data[SensorData::tailPan] > 0)
    00516       jointData.data[JointData::tailPan] = 380000;
    00517     else
    00518       jointData.data[JointData::tailPan] = -380000;
    00519     break;
    00520   case TailRequest::fivePositionSwitch:
    00521     if(sensorData.data[SensorData::tailTilt] > -130000 &&
    00522        sensorData.data[SensorData::tailTilt] < 130000 &&
    00523        sensorData.data[SensorData::tailPan] > -130000 &&
    00524        sensorData.data[SensorData::tailPan] < 130000)
    00525     {
    00526       jointData.data[JointData::tailPan] = 0;
    00527       jointData.data[JointData::tailTilt] = 0;
    00528     }
    00529     else
    00530     {
    00531       if (sensorData.data[SensorData::tailTilt] > 0)
    00532         jointData.data[JointData::tailTilt] = 380000;
    00533       else
    00534         jointData.data[JointData::tailTilt] = -380000;
    00535
    00536       if (sensorData.data[SensorData::tailPan] > 0)
    00537         jointData.data[JointData::tailPan] = 380000;
    00538       else
    00539         jointData.data[JointData::tailPan] = -380000;
    00540     }
    00541     break;
    00542   case TailRequest::eightPositionSwitch:
    00543     // vertical
    00544     jointData.data[JointData::tailPan] = 0;
    00545     if (sensorData.data[SensorData::tailTilt] > 130000)
    00546     {
    00547       jointData.data[JointData::tailTilt] = 380000;
    00548       if (sensorData.data[SensorData::tailPan] > 130000)
    00549         jointData.data[JointData::tailPan] = 380000;
    00550       else if(sensorData.data[SensorData::tailPan] > -130000)
    00551         jointData.data[JointData::tailPan] = 0;
    00552       else
    00553         jointData.data[JointData::tailPan] = -380000;
    00554     }
    00555     else if(sensorData.data[SensorData::tailTilt] > -130000)
    00556     {
    00557       jointData.data[JointData::tailTilt] = 0;
    00558       if (sensorData.data[SensorData::tailPan] > 130000)
    00559         jointData.data[JointData::tailPan] = 380000;
    00560       else if(sensorData.data[SensorData::tailPan] > -130000)
    00561       {
    00562         // if the tail is in the center it is set to the top
    00563         jointData.data[JointData::tailPan] = 0;
    00564         jointData.data[JointData::tailTilt] = 380000;
    00565       }
    00566       else
    00567         jointData.data[JointData::tailPan] = -380000;
    00568     }
    00569     else
    00570     {
    00571       jointData.data[JointData::tailTilt] = -380000;
    00572       if (sensorData.data[SensorData::tailPan] > 130000)
    00573         jointData.data[JointData::tailPan] = 380000;
    00574       else if(sensorData.data[SensorData::tailPan] > -130000)
    00575         jointData.data[JointData::tailPan] = 0;
    00576       else
    00577         jointData.data[JointData::tailPan] = -380000;
    00578     }
    00579     break;
    00580   case TailRequest::tailLeftTop:
    00581     jointData.data[JointData::tailPan] = -380000;
    00582     jointData.data[JointData::tailTilt] = 380000;
    00583     break;
    00584   case TailRequest::tailCenterTop:
    00585     jointData.data[JointData::tailPan] =       0;
    00586     jointData.data[JointData::tailTilt] = 380000;
    00587     break;
    00588   case TailRequest::tailRightTop:
    00589     jointData.data[JointData::tailPan] =  380000;
    00590     jointData.data[JointData::tailTilt] = 380000;
    00591     break;
    00592   case TailRequest::tailLeftCenter:
    00593     jointData.data[JointData::tailPan] = -380000;
    00594     jointData.data[JointData::tailTilt] =      0;
    00595     break;
    00596   case TailRequest::tailCenterCenter:
    00597     jointData.data[JointData::tailPan] =       0;
    00598     jointData.data[JointData::tailTilt] =      0;
    00599     break;
    00600   case TailRequest::tailRightCenter:
    00601     jointData.data[JointData::tailPan] =  380000;
    00602     jointData.data[JointData::tailTilt] =      0;
    00603     break;
    00604   case TailRequest::tailLeftBottom:
    00605     jointData.data[JointData::tailPan] = -380000;
    00606     jointData.data[JointData::tailTilt] = -380000;
    00607     break;
    00608   case TailRequest::tailCenterBottom:
    00609     jointData.data[JointData::tailPan] =        0;
    00610     jointData.data[JointData::tailTilt] = -380000;
    00611     break;
    00612   case TailRequest::tailRightBottom:
    00613     jointData.data[JointData::tailPan] = 380000;
    00614     jointData.data[JointData::tailTilt] = -380000;
    00615     break;
    00616   }
    00617 }
    00618
    00619 bool GT2005MotionControl::handleMessage(InMessage& message)
    00620 {
    00621   switch(message.getMessageID())
    00622   {
    00623   case idGenericDebugData:
    00624     {
    00625       GenericDebugData d;
    00626       message.bin >> d;
    00627       if(d.id == GenericDebugData::motionStabilizer)
    00628       {
    00629         OUTPUT(idText,text,"generic debug message (motionStabilizer) handled by module motionStabilizer");
    00630
    00631         xFore.setWeightP(d.data[0]);
    00632         xHind.setWeightP(d.data[0]);
    00633         yLeft.setWeightP(d.data[1]);
    00634         yRight.setWeightP(d.data[1]);
    00635
    00636         stabilizerScale = d.data[2];
    00637       }
    00638     }
    00639     break;
    00640   case idMotionNet:
    00641     return pSpecialActions->handleMessage(message);
    00642   case idOdometryScale:
    00643     return pWalkingEngine[WalkRequest::normal]->handleMessage(message);
    00644   case idInvKinWalkingParameters:
    00645     return pWalkingEngine[WalkRequest::debug]->handleMessage(message);
    00646   case idGT2005Request:
    00647     return pWalkingEngine[WalkRequest::normal]->handleMessage(message);
    00648   }
    00649   return false;
    00650 }