Modules/MotionControl/GT2004MotionControl.cpp

Go to the documentation of this file.

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

#include "GT2004MotionControl.h"

#include "Modules/WalkingEngine/WalkingEngineSelector.h"
#include "Modules/WalkingEngine/InvKinWalkingEngine.h"
#include "Modules/WalkingEngine/InvKinWalkingParameterSets.h"
#include "Modules/SpecialActions/GT2003MotionNetSpecialActions/GT2003MotionNetSpecialActions.h"

#include "Platform/SystemCall.h"
#include "Tools/Actorics/Kinematics.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/RobotConfiguration.h"

GT2004MotionControl::GT2004MotionControl(ModuleHandler& moduleHandler,
                                           const MotionControlInterfaces& interfaces) 
                                           :MotionControl(interfaces),
                                           moduleHandler(moduleHandler),
                                           changeOfMotionControlStatePossible(true),
                                           lastMotionType(MotionRequest::playDead),
                                           motionControlState(GT2004MotionControl::playDead)
{
  // create an uninitialised motion request to set startup motion -- something like getup (see MotionRequest-Constructor)
  MotionRequest defaultRequest;
  // currentMotionType = lastMotionType = defaultRequest.motionType;
  lastHeadTilt = lastHeadPan = lastHeadRoll = 0;

  WalkingEngineInterfaces walkingEngineInterfaces
    (sensorDataBuffer, invKinWalkingParameters, gt2004Parameters, walkParameterTimeStamp, receivedNewSensorData, lastMotionType, pidData, odometryData, motionInfo);

  // create the InvKinWalking engine as only one instance is created
  // and used by different walk type with different parameter sets
  pInvKinWalkingEngine = new InvKinWalkingEngine(walkingEngineInterfaces);
  
  
  pWalkingEngine[WalkRequest::normal] = 
    new WalkingEngineSelector(moduleHandler, SolutionRequest::walkingEngineNormal,
    walkingEngineInterfaces, pInvKinWalkingEngine);
  
  pWalkingEngine[WalkRequest::upsideDown] = 
    new ParamInvKinWalkingEngine(new UpsideDownWalkingParameters, pInvKinWalkingEngine);
  
  pWalkingEngine[WalkRequest::turnWithBall] = 
    new ParamInvKinWalkingEngine(new TurnWithBallWalkingParameters, pInvKinWalkingEngine);
  
  pWalkingEngine[WalkRequest::turnKick] = 
    new ParamInvKinWalkingEngine(new TurnWithBallWalkingParameters, pInvKinWalkingEngine);
  
  pWalkingEngine[WalkRequest::dash] =
    new BB2004InvKinWalkingEngine(pInvKinWalkingEngine);
  
  pWalkingEngine[WalkRequest::debug] = 
    new ParamInvKinWalkingEngine(new ERS7EvolveWalkingParameters, pInvKinWalkingEngine);
  
  SpecialActionsInterfaces specialActionsInterfaces(motionRequest,lastMotionType, 
    pidData, odometryData, motionInfo);
  pSpecialActions = new GT2003MotionNetSpecialActions(specialActionsInterfaces);
  
  GetupEngineInterfaces getupEngineInterfaces(lastMotionType, sensorDataBuffer, motionRequest, bodyPosture, pidData, odometryData,
    motionInfo);
  pGetupEngine = new GetupEngineSelector(moduleHandler,getupEngineInterfaces);
  
  wakingUp = false;
  // lastSpecialActionWasPlayDead = false;
}

GT2004MotionControl::~GT2004MotionControl()
{
  delete pWalkingEngine[WalkRequest::normal];
  delete pWalkingEngine[WalkRequest::upsideDown];
  delete pWalkingEngine[WalkRequest::turnWithBall];
  delete pWalkingEngine[WalkRequest::turnKick];
  delete pWalkingEngine[WalkRequest::dash];
  delete pWalkingEngine[WalkRequest::debug];
  delete pSpecialActions;
  delete pGetupEngine;
  delete pInvKinWalkingEngine;
  
  moduleHandler.setModuleSelector(SolutionRequest::walkingEngineNormal,0);
  moduleHandler.setModuleSelector(SolutionRequest::getupEngine,0);
}

void GT2004MotionControl::determineMotionControlState()
{
  switch(motionControlState)
  {
  case GT2004MotionControl::playDead:
    {
      if(motionRequest.motionType != MotionRequest::playDead)
      {
        motionControlState = GT2004MotionControl::wakeUp;
      }
      return;
    }
  case GT2004MotionControl::getup:
    if(changeOfMotionControlStatePossible)
    {
        setMotionControlState();
      
    }
    return;
  case GT2004MotionControl::wakeUp:
    if(changeOfMotionControlStatePossible)
    {
      setMotionControlState();
      
    }
    return;
  case GT2004MotionControl::stand:
    if(changeOfMotionControlStatePossible)
    {
      setMotionControlState();
    }
    return;
  case GT2004MotionControl::specialAction:
    if(changeOfMotionControlStatePossible)
    {
      setMotionControlState();
    }
    return;
  case GT2004MotionControl::walk:
    if(
      changeOfMotionControlStatePossible && 
      ((motionRequest.motionType != MotionRequest::specialAction || 
        pSpecialActions->specialActionIsExecutableInWalkingCycle(motionRequest.specialActionRequest.specialActionType, positionInWalkCycle))
      ))
    {
      setMotionControlState();
    }
    return;
  }
}

void GT2004MotionControl::setMotionControlState()
{
  switch(motionRequest.motionType)
  {
  case MotionRequest::playDead:
    motionControlState = GT2004MotionControl::playDead;
    return;
  case MotionRequest::getup:
    motionControlState = GT2004MotionControl::getup;
    return;
  case MotionRequest::stand:
    motionControlState = GT2004MotionControl::stand;
    return;
  case MotionRequest::specialAction:
    motionControlState = GT2004MotionControl::specialAction;
    return;
  case MotionRequest::walk:
    motionControlState = GT2004MotionControl::walk;
    return;
  }
}
void GT2004MotionControl::execute()
{
  for (int i = 0; i < jointDataBufferNumOfFrames; i++)
  {
  
    JointData &currentFrame = jointDataBuffer.frame[i];

    // remember some variables
    if(motionRequest.motionType == MotionRequest::walk)
      latestWalkRequest = motionRequest.walkRequest;
    else if(motionRequest.motionType == MotionRequest::specialAction)
      latestSpecialActionRequest = motionRequest.specialActionRequest;

    
    // determine current state for Motion Control
    determineMotionControlState();
    
    /* state machine for current action*/
    switch(motionControlState)
    {
    case GT2004MotionControl::playDead:
      {
        changeOfMotionControlStatePossible = true;
        for(int i = 0; i<JointData::numOfJoint; ++i)
        {
          pidData.p[i]=0;pidData.i[i]=0;pidData.d[i]=0;
        }
        motionInfo.executedMotionRequest.motionType = MotionRequest::playDead;
        lastMotionType = MotionRequest::playDead;
        break;
      }
    case GT2004MotionControl::wakeUp:
      {
        changeOfMotionControlStatePossible
          = !wakeUpEngine.execute(frameNumber, sensorDataBuffer.frame[0], currentFrame, pidData);
        lastMotionType = MotionRequest::playDead;
        break;
      }
      case GT2004MotionControl::getup:
        {
          changeOfMotionControlStatePossible = !pGetupEngine->executeParameterized(currentFrame);
          lastMotionType = MotionRequest::getup;
          break;
        }
    case GT2004MotionControl::stand:
      {
        WalkRequest standRequest;
        standRequest.walkType = WalkRequest::normal;
        standRequest.walkParams = Pose2D(0,0,0);
        changeOfMotionControlStatePossible 
          = !pWalkingEngine[WalkRequest::normal]->executeParameterized(currentFrame,standRequest,0);
        motionInfo.executedMotionRequest.motionType = MotionRequest::stand;
        positionInWalkCycle = motionInfo.positionInWalkCycle;
        lastMotionType = MotionRequest::stand;
        break;
      }
      
    case GT2004MotionControl::specialAction:
      changeOfMotionControlStatePossible 
        = !pSpecialActions->executeParameterized(latestSpecialActionRequest, currentFrame);
      lastMotionType = MotionRequest::specialAction;
      break;
      
    case GT2004MotionControl::walk:
      {
        changeOfMotionControlStatePossible 
          = !pWalkingEngine[latestWalkRequest.walkType]->executeParameterized(currentFrame, latestWalkRequest, positionInWalkCycle);
        positionInWalkCycle = motionInfo.positionInWalkCycle;
        lastMotionType = MotionRequest::walk;
        break;
      }
    }

    if(motionControlState != GT2004MotionControl::walk){
      positionInWalkCycle = 0;
    }
    
    /* end of state machine*/
    
    headIsBlockedBySpecialActionOrWalk = 
      (
      currentFrame.data[JointData::neckTilt] != jointDataInvalidValue &&
      currentFrame.data[JointData::headPan] != jointDataInvalidValue &&
      currentFrame.data[JointData::headTilt] != jointDataInvalidValue 
      );
    
    // execute HeadMotionRequest (smoothed)
    const long maxdiff=50000; //(~2pi/s)
    long diff;
    if (currentFrame.data[JointData::neckTilt] == jointDataInvalidValue)
    {
      diff=headMotionRequest.tilt-lastHeadTilt;
      if (diff<-maxdiff)
      {
        currentFrame.data[JointData::neckTilt] = lastHeadTilt-maxdiff;
      }
      else if (diff<maxdiff)
      {
        currentFrame.data[JointData::neckTilt] = headMotionRequest.tilt;
      }
      else
      {
        currentFrame.data[JointData::neckTilt] = lastHeadTilt+maxdiff;
      }
    }
    if (currentFrame.data[JointData::headPan] == jointDataInvalidValue)
    {
      diff=headMotionRequest.pan-lastHeadPan;
      if (diff<-maxdiff)
      {
        currentFrame.data[JointData::headPan] = lastHeadPan-maxdiff;
      }
      else if (diff<maxdiff)
      {
        currentFrame.data[JointData::headPan] = headMotionRequest.pan;
      }
      else
      {
        currentFrame.data[JointData::headPan] = lastHeadPan+maxdiff;
      }
    }
    if (currentFrame.data[JointData::headTilt] == jointDataInvalidValue)
    {
      diff=headMotionRequest.roll-lastHeadRoll;
      if (diff<-maxdiff)
      {
        currentFrame.data[JointData::headTilt] = lastHeadRoll-maxdiff;
      }
      else if (diff<maxdiff)
      {
        currentFrame.data[JointData::headTilt] = headMotionRequest.roll;
      }
      else
      {
        currentFrame.data[JointData::headTilt] = lastHeadRoll+maxdiff;
      }
    }
    if (currentFrame.data[JointData::mouth] == jointDataInvalidValue)
    {
      currentFrame.data[JointData::mouth] = headMotionRequest.mouth;
    }
    
    lastHeadTilt=currentFrame.data[JointData::neckTilt];
    lastHeadPan =currentFrame.data[JointData::headPan];
    lastHeadRoll=currentFrame.data[JointData::headTilt];
    
    // the obgligatory GT tail wag
    /** @todo:
    wagTail(sensorDataBuffer.lastFrame(), currentFrame);
    */
    
    // whether or not stabilize does something is determined in the method.
    // it is called because of averaging that needs to be done continously!!
    stabilize(lastMotionType, motionRequest, currentFrame, odometryData, sensorDataBuffer);
  }
}

void GT2004MotionControl::wagTail(
                                   const MotionRequest& motionRequest,
                                   const SensorData& sensorData,
                                   JointData& jointData
                                   )
{
  if ((jointData.data[JointData::tailPan] != jointDataInvalidValue) ||
    (jointData.data[JointData::tailTilt] != jointDataInvalidValue))
    return;
  
  switch (motionRequest.tailRequest.tailRequestID)
  {
  case TailRequest::wagHorizontal:
    {
      int framesPerPeriod = 200;
      tailCount %= framesPerPeriod;
      tailCount++;
      jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanP * sin(tailCount * 2 * pi / framesPerPeriod));
      jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltN);
    }
    break;
  case TailRequest::wagLeftRightHorizontal:
    {
      int framesPerPeriod = 125;
      tailCount %= framesPerPeriod;
      tailCount++;
      jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanP * sin(tailCount * 2 * pi / framesPerPeriod));
      jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltN);
    }
    break;
  case TailRequest::wagLeftRightVertical:
    {
      int framesPerPeriod = 125;
      tailCount %= framesPerPeriod;
      tailCount++;
      jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanP * sin(tailCount * 2 * pi / framesPerPeriod));
      jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltN);
    }
    break;
  case TailRequest::wagHorizontalFast:
    {
      int framesPerPeriod = 30;
      tailCount %= framesPerPeriod;
      tailCount++;
      jointData.data[JointData::tailPan] = toMicroRad((jointLimitTailPanP/2) * sgn(sin(tailCount * 2 * pi / framesPerPeriod)));
      jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltN);
    }
    break;
  case TailRequest::wagVertical:
    {
      int framesPerPeriod = 125;
      tailCount %= framesPerPeriod;
      tailCount++;
      jointData.data[JointData::tailTilt] = 
        toMicroRad(
        (jointLimitTailTiltP + jointLimitTailTiltN) / 2 +
        (jointLimitTailTiltP - jointLimitTailTiltN) / 2 * sin(tailCount * 2 * pi / framesPerPeriod)
        );
      jointData.data[JointData::tailPan] = 0;
    }
    break;
  case TailRequest::wagUpDownLeft:
    {
      int framesPerPeriod = 125;
      tailCount %= framesPerPeriod;
      tailCount++;
      jointData.data[JointData::tailTilt] = 
        toMicroRad(
        (jointLimitTailTiltP + jointLimitTailTiltN) / 2 +
        (jointLimitTailTiltP - jointLimitTailTiltN) / 2 * sin(tailCount * 2 * pi / framesPerPeriod)
        );
      jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanN);
    }
    break;
  case TailRequest::wagUpDownRight:
    {
      int framesPerPeriod = 125;
      tailCount %= framesPerPeriod;
      tailCount++;
      jointData.data[JointData::tailTilt] = 
        toMicroRad(
        (jointLimitTailTiltP + jointLimitTailTiltN) / 2 +
        (jointLimitTailTiltP - jointLimitTailTiltN) / 2 * sin(tailCount * 2 * pi / framesPerPeriod)
        );
      jointData.data[JointData::tailPan] = toMicroRad(jointLimitTailPanP);
    }
    break;
  case TailRequest::wagVerticalFast:
    {
      tailCount &= 7;
      int j=tailCount++;
      if (j>3) j = 8-j;
      jointData.data[JointData::tailTilt] = 160000*(j - 2);
      jointData.data[JointData::tailPan] = 0;
    }
    break;
  case TailRequest::stayAsForced:
    jointData.data[JointData::tailTilt] = sensorData.data[SensorData::tailTilt];
    jointData.data[JointData::tailPan] = sensorData.data[SensorData::tailPan];
    break;
  case TailRequest::stayAsForcedPan:
    jointData.data[JointData::tailTilt] = toMicroRad(jointLimitTailTiltP);
    jointData.data[JointData::tailPan] = sensorData.data[SensorData::tailPan];
    break;
  case TailRequest::stayAsForcedTilt:
    jointData.data[JointData::tailTilt] = sensorData.data[SensorData::tailTilt];
    jointData.data[JointData::tailPan] = 0;
    break;
  case TailRequest::tailFollowsHead:
    jointData.data[JointData::tailTilt] = sensorData.data[SensorData::neckTilt];
    jointData.data[JointData::tailPan] = -sensorData.data[SensorData::headPan];
    break;
  case TailRequest::tailParallelToGround:
    jointData.data[JointData::tailTilt] = 0;
    jointData.data[JointData::tailPan] = 0;
    break;
  case TailRequest::noTailWag:
    jointData.data[JointData::tailTilt] = 0;
    jointData.data[JointData::tailPan] = 0;
    break;
  case TailRequest::twoPositionSwitchVertical:
    jointData.data[JointData::tailPan] = 0;
    if (sensorData.data[SensorData::tailTilt] > 0)
      jointData.data[JointData::tailTilt] = 380000;
    else
      jointData.data[JointData::tailTilt] = -380000;
    break;
  case TailRequest::twoPositionSwitchHorizontal:
    jointData.data[JointData::tailTilt] = 0;
    if (sensorData.data[SensorData::tailPan] > 0)
      jointData.data[JointData::tailPan] = 380000;
    else
      jointData.data[JointData::tailPan] = -380000;
    break;
  case TailRequest::threePositionSwitchVertical:
    jointData.data[JointData::tailPan] = 0;
    if (sensorData.data[SensorData::tailTilt] > 130000)
      jointData.data[JointData::tailTilt] = 380000;
    else if(sensorData.data[SensorData::tailTilt] > -130000)
      jointData.data[JointData::tailTilt] = 0;
    else
      jointData.data[JointData::tailTilt] = -380000;
    break;
  case TailRequest::threePositionSwitchHorizontal:
    jointData.data[JointData::tailTilt] = 0;
    if (sensorData.data[SensorData::tailPan] > 130000)
      jointData.data[JointData::tailPan] = 380000;
    else if(sensorData.data[SensorData::tailPan] > -130000)
      jointData.data[JointData::tailPan] = 0;
    else
      jointData.data[JointData::tailPan] = -380000;
    break;
  case TailRequest::fourPositionSwitchUpDownLeftRight:
    if(sensorData.data[SensorData::tailPan] > sensorData.data[SensorData::tailTilt])
    {
      if(sensorData.data[SensorData::tailPan] > -sensorData.data[SensorData::tailTilt])
      {
        //right
        jointData.data[JointData::tailPan] = 380000;
        jointData.data[JointData::tailTilt] = 0;
      }
      else
      {
        //bottom
        jointData.data[JointData::tailPan] = 0;
        jointData.data[JointData::tailTilt] = -380000;
      }
    }
    else
    {
      if(sensorData.data[SensorData::tailPan] > -sensorData.data[SensorData::tailTilt])
      {
        //top
        jointData.data[JointData::tailPan] = 0;
        jointData.data[JointData::tailTilt] = 380000;
      }
      else
      {
        //left
        jointData.data[JointData::tailPan] = -380000;
        jointData.data[JointData::tailTilt] = 0;
      }
    }
    break;
  case TailRequest::fourPositionSwitchCorners:
    if (sensorData.data[SensorData::tailTilt] > 0)
      jointData.data[JointData::tailTilt] = 380000;
    else
      jointData.data[JointData::tailTilt] = -380000;

    if (sensorData.data[SensorData::tailPan] > 0)
      jointData.data[JointData::tailPan] = 380000;
    else
      jointData.data[JointData::tailPan] = -380000;
    break;
  case TailRequest::fivePositionSwitch:
    if(sensorData.data[SensorData::tailTilt] > -130000 &&
       sensorData.data[SensorData::tailTilt] < 130000 &&
       sensorData.data[SensorData::tailPan] > -130000 &&
       sensorData.data[SensorData::tailPan] < 130000)
    {
      jointData.data[JointData::tailPan] = 0;
      jointData.data[JointData::tailTilt] = 0;
    }
    else
    {
      if (sensorData.data[SensorData::tailTilt] > 0)
        jointData.data[JointData::tailTilt] = 380000;
      else
        jointData.data[JointData::tailTilt] = -380000;

      if (sensorData.data[SensorData::tailPan] > 0)
        jointData.data[JointData::tailPan] = 380000;
      else
        jointData.data[JointData::tailPan] = -380000;
    }
    break;
  case TailRequest::eightPositionSwitch:
    // vertical
    jointData.data[JointData::tailPan] = 0;
    if (sensorData.data[SensorData::tailTilt] > 130000)
    {
      jointData.data[JointData::tailTilt] = 380000;
      if (sensorData.data[SensorData::tailPan] > 130000)
        jointData.data[JointData::tailPan] = 380000;
      else if(sensorData.data[SensorData::tailPan] > -130000)
        jointData.data[JointData::tailPan] = 0;
      else
        jointData.data[JointData::tailPan] = -380000;
    }
    else if(sensorData.data[SensorData::tailTilt] > -130000)
    {
      jointData.data[JointData::tailTilt] = 0;
      if (sensorData.data[SensorData::tailPan] > 130000)
        jointData.data[JointData::tailPan] = 380000;
      else if(sensorData.data[SensorData::tailPan] > -130000)
      {
        // if the tail is in the center it is set to the top
        jointData.data[JointData::tailPan] = 0;
        jointData.data[JointData::tailTilt] = 380000;
      }
      else
        jointData.data[JointData::tailPan] = -380000;
    }
    else
    {
      jointData.data[JointData::tailTilt] = -380000;
      if (sensorData.data[SensorData::tailPan] > 130000)
        jointData.data[JointData::tailPan] = 380000;
      else if(sensorData.data[SensorData::tailPan] > -130000)
        jointData.data[JointData::tailPan] = 0;
      else
        jointData.data[JointData::tailPan] = -380000;
    }
    break;
  case TailRequest::tailLeftTop:
    jointData.data[JointData::tailPan] = -380000;
    jointData.data[JointData::tailTilt] = 380000;
    break;
  case TailRequest::tailCenterTop:
    jointData.data[JointData::tailPan] =       0;
    jointData.data[JointData::tailTilt] = 380000;
    break;
  case TailRequest::tailRightTop:
    jointData.data[JointData::tailPan] =  380000;
    jointData.data[JointData::tailTilt] = 380000;
    break;
  case TailRequest::tailLeftCenter:
    jointData.data[JointData::tailPan] = -380000;
    jointData.data[JointData::tailTilt] =      0;
    break;
  case TailRequest::tailCenterCenter:
    jointData.data[JointData::tailPan] =       0;
    jointData.data[JointData::tailTilt] =      0;
    break;
  case TailRequest::tailRightCenter:
    jointData.data[JointData::tailPan] =  380000;
    jointData.data[JointData::tailTilt] =      0;
    break;
  case TailRequest::tailLeftBottom:
    jointData.data[JointData::tailPan] = -380000;
    jointData.data[JointData::tailTilt] = -380000;
    break;
  case TailRequest::tailCenterBottom:
    jointData.data[JointData::tailPan] =        0;
    jointData.data[JointData::tailTilt] = -380000;
    break;
  case TailRequest::tailRightBottom:
    jointData.data[JointData::tailPan] = 380000;
    jointData.data[JointData::tailTilt] = -380000;
    break;
  }
}

bool GT2004MotionControl::handleMessage(InMessage& message)
{ 
  switch(message.getMessageID())
  {
  case idGenericDebugData:
    {
      GenericDebugData d;
      message.bin >> d;
      if(d.id == GenericDebugData::motionStabilizer)
      {
        OUTPUT(idText,text,"generic debug message (motionStabilizer) handled by module motionStabilizer");
        
        xFore.setWeightP(d.data[0]);
        xHind.setWeightP(d.data[0]);
        yLeft.setWeightP(d.data[1]);
        yRight.setWeightP(d.data[1]);
        
        stabilizerScale = d.data[2];
      }
    }
    break;
  case idMotionNet:
    return pSpecialActions->handleMessage(message);
  case idOdometryScale:
    return pWalkingEngine[WalkRequest::normal]->handleMessage(message);
  case idInvKinWalkingParameters:
    return pWalkingEngine[WalkRequest::debug]->handleMessage(message);
  }
  return false;
}


/*
* Change log :
* 
* $Log: GT2004MotionControl.cpp,v $
* Revision 1.1  2004/07/10 00:18:29  spranger
* renamed (readded) for coderelease
*
* Revision 1.20  2004/06/16 18:11:30  spranger
* bugfix
*
* Revision 1.19  2004/06/16 16:52:28  risler
* no more change to getup before wakeup is finished
*
* Revision 1.18  2004/06/16 16:15:52  spranger
* debug-stuff added
*
* Revision 1.17  2004/06/14 16:55:10  juengel
* Removed some WalkingEngineParameterSets.
*
* Revision 1.16  2004/06/14 15:28:36  spranger
* removed some more debug-stuff
*
* Revision 1.15  2004/06/14 15:18:47  spranger
* removed some debug-stuff
*
* Revision 1.14  2004/06/14 15:05:52  spranger
* redone statemachine, changed some interfaces
*
* Revision 1.13  2004/06/08 15:29:09  juengel
* Added some OUTPUTs.
*
* Revision 1.12  2004/06/07 18:52:21  spranger
* added positionInWalkCycle, added functionality for preserving positionInwalkCycle and WalkEngine-Control
*
* Revision 1.11  2004/06/05 10:02:31  roefer
* Memory leak closed
*
* Revision 1.10  2004/06/04 16:36:43  juengel
* Added walkType turn-kick
*
* Revision 1.9  2004/06/04 13:46:10  dueffert
* unused turnwithball walk types removed
*
* Revision 1.8  2004/06/02 17:19:31  spranger
* cleanup
*
* Revision 1.7  2004/05/27 18:20:49  spranger
* motionControl cleaned up
*
* Revision 1.6  2004/05/27 17:13:37  jhoffman
* - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
* - clipping included for setJoints
* - removed some microrad/rad-bugs
* - bodyPosture constructor and "=" operator fixed
*
* Revision 1.5  2004/05/27 09:29:28  loetzsch
* removed executedMotionRequest from Interfaces
*
* Revision 1.4  2004/05/27 09:13:04  loetzsch
* removed executedMotionRequest from Interfaces
*
* Revision 1.3  2004/05/26 20:17:47  juengel
* Removed RobotVerticesBuffer.
* Added "receivedNewSensorData".
*
* Revision 1.2  2004/05/26 16:10:24  dueffert
* better data types used
*
* Revision 1.1.1.1  2004/05/22 17:20:36  cvsadm
* created new repository GT2004_WM
*
* Revision 1.27  2004/05/22 14:28:56  juengel
* Activated WakeUpEngine
*
* Revision 1.26  2004/05/19 15:26:58  juengel
* WakeUpEngine
*
* Revision 1.25  2004/05/19 13:33:21  heinze
* added conditions for WakeUpEngine
*
* Revision 1.24  2004/05/03 09:51:18  heinze
* Prepared WakeUpEngine.
*
* Revision 1.23  2004/04/08 15:33:06  wachter
* GT04 checkin of Microsoft-Hellounds
*
* Revision 1.22  2004/04/07 12:28:59  risler
* ddd checkin after go04 - first part
*
* Revision 1.21  2004/03/29 17:59:29  dueffert
* sorry
*
* Revision 1.20  2004/03/29 15:24:39  dueffert
* new turn parameterset
*
* Revision 1.19  2004/03/26 21:29:44  roefer
* Headstate handling improved
*
* Revision 1.18  2004/03/26 10:01:47  dueffert
* warning removed
*
* Revision 1.17  2004/03/25 09:11:57  jhoffman
* breathing motion added (robot opens/closes mouth)
*
* Revision 1.16  2004/03/22 21:58:11  roefer
* True odometry
*
* Revision 1.15  2004/03/20 09:55:25  roefer
* Preparation for improved odometry
*
* Revision 1.14  2004/03/19 11:25:01  juengel
* Added walkType turnWithBall2.
*
* Revision 1.13  2004/03/17 21:08:39  cesarz
* Added turnWithBall3, which points to MSH2004TurnWithBallWalkingParameters.
*
* Revision 1.12  2004/03/17 15:38:51  thomas
* added walkType debug for debugging WalkingEngineParameterSets
* paramSets-messages send by debug-message are stored in the new debug-walkType only
*
* Revision 1.11  2004/03/17 09:32:11  jhoffman
* tail wag adjusted to work with ERS7
*
* Revision 1.10  2004/03/08 01:39:02  roefer
* Interfaces should be const
*
* Revision 1.9  2004/03/04 18:15:24  juengel
* removed unused comment
*
* Revision 1.8  2004/02/16 18:02:01  dueffert
* packageCognitionMotion extended with invkin parameters
*
* Revision 1.7  2004/01/21 14:31:58  loetzsch
* Module Selectors create only the selected solution.
* When the solution changes, the old solution is erased and the new
* one ist created using createSolution(..)
*
* Revision 1.6  2004/01/05 17:56:27  juengel
* Added HeadControlModes stayAsForcedPan and stayAsForcedTilt.
*
* Revision 1.5  2004/01/05 11:37:08  juengel
* Added new TailModes: wagUpDown* and wagLeftRight*.
*
* Revision 1.4  2003/12/31 23:50:38  roefer
* Removed inclusion of RobotDimensions.h because it is not used
*
* Revision 1.3  2003/12/16 19:02:45  loetzsch
* The motion net file Config/spec_act.dat can be sent through WLAN to a robot.
*
* Revision 1.2  2003/12/10 16:52:52  roefer
* Added sensor data buffer to walking engine
*
* Revision 1.1  2003/10/06 14:10:13  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.6  2003/09/26 11:38:52  juengel
* - sorted tools
* - clean-up in DataTypes
*
* Revision 1.5  2003/09/25 10:11:57  juengel
* Removed some walk-types
*
* Revision 1.4  2003/08/04 07:54:39  risler
* no message
*
* Revision 1.3  2003/07/24 12:41:26  risler
* ParamInvKinWalkingEngine no longer is template class -> 10min boot bug fixed
*
* Revision 1.2  2003/07/02 15:42:32  loetzsch
* esoteric change (but it works now)
*
* Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.38  2003/06/13 17:29:25  jhoffman
* added debug message handling
*
* Revision 1.37  2003/05/27 16:17:58  juengel
* Added headIsBlockedBySpecialActionOrWalk.
*
* Revision 1.36  2003/05/26 16:23:37  dueffert
* no message
*
* Revision 1.35  2003/05/23 11:30:12  goehring
* MotionStabilizer signature changed
*
* Revision 1.34  2003/05/23 09:36:51  goehring
* MotionStabilizer gets all 4 Sensor Values
*
* Revision 1.33  2003/05/06 16:52:30  juengel
* Added tail-mode eightPositionSwitch.
*
* Revision 1.32  2003/05/02 18:26:18  risler
* SensorDataBuffer added
* replaced SensorData with SensorDataBuffer
* full SensorData resolution now accessible
*
* Revision 1.31  2003/04/28 07:43:57  roefer
* Error in head motion smoothing fixed
*
* Revision 1.30  2003/04/27 07:49:35  roefer
* static vars -> member vars
*
* Revision 1.29  2003/04/19 04:36:48  pruente
* Commited changes done by Arthur at GO2003:
* - some changes
* - some changes
* - dash chanaged to evo parameters
*
* Revision 1.28  2003/04/16 07:00:16  roefer
* Bremen GO checkin
*
* Revision 1.28  2003/04/08 18:28:28  roefer
* bodyTiltOffset added
*
* Revision 1.27  2003/04/02 14:13:39  dueffert
* walkType turnWithBall added
*
* Revision 1.26  2003/04/01 17:44:21  roefer
* Mouth added to HeadMotionRequest
*
* Revision 1.25  2003/03/24 14:46:21  juengel
* Added several tail modes
*
* Revision 1.24  2003/03/10 14:18:39  dueffert
* optimized MotionNet
*
* Revision 1.23  2003/03/06 12:05:43  dueffert
* execute with parameters renamed to avoid inheritance warnings
*
* Revision 1.22  2003/03/03 17:31:53  risler
* walking engine now writes calculated bodyTilt to headState
*
* Revision 1.21  2003/02/22 00:12:07  roefer
* constant bodyTilt set to 17.5°
*
* Revision 1.20  2003/02/16 08:30:38  roefer
* Constant body tilt changed to 17°
*
* Revision 1.19  2003/02/11 23:12:50  roefer
* useConstantBodyPose debug key added
*
* Revision 1.18  2003/02/06 13:15:14  dueffert
* memory leak fixed
*
* Revision 1.17  2003/02/05 12:23:32  dueffert
* executedMotionRequest is set correctly by SpecialAction and therefore not overwritten by DefaultMotionControl any more
*
* Revision 1.16  2003/01/27 19:27:08  juengel
* added walkType upsideDown
*
* Revision 1.15  2003/01/23 16:44:09  risler
* only one instance of InvKinWalkingEngine
* parameter sets can now be switched while walking
* added UNSWFastTurn, combining two parameter sets
*
* Revision 1.14  2002/12/03 13:07:02  dueffert
* HeadMotionRequests smoothed to avoid destroying heads
*
* Revision 1.13  2002/11/26 17:18:13  risler
* breathing only when standing is executed (not requested)
*
* Revision 1.12  2002/11/26 13:08:05  jhoffman
* no message
*
* Revision 1.11  2002/11/25 14:49:07  jhoffman
* added "breathing" motion
*
* Revision 1.10  2002/11/20 18:02:29  risler
* added PID values to GT2001MotionNetSpecialActions
* added playDead motion
*
* Revision 1.9  2002/11/19 17:12:24  juengel
* "switch" tail modes added
*
* Revision 1.8  2002/10/11 16:59:15  roefer
* Fast tail wags added
*
* Revision 1.7  2002/09/22 18:40:53  risler
* added new math functions, removed GTMath library
*
* Revision 1.6  2002/09/19 12:15:13  loetzsch
* made compilable for the gcc compiler.
*
* Revision 1.5  2002/09/17 23:55:21  loetzsch
* - unraveled several datatypes
* - changed the WATCH macro
* - completed the process restructuring
*
* Revision 1.4  2002/09/16 17:36:04  dueffert
* anonymous contructors return &CLASS with VS, but CLASS with gcc.
*
* Revision 1.3  2002/09/11 13:40:00  loetzsch
* Changed DoxyGen comments
*
* Revision 1.2  2002/09/11 00:06:58  loetzsch
* continued change of module/solution mechanisms
*
* Revision 1.1  2002/09/10 15:36:15  cvsadm
* Created new project GT2003 (M.L.)
* - Cleaned up the /Src/DataTypes directory
* - Removed challenge related source code
* - Removed processing of incoming audio data
* - Renamed AcousticMessage to SoundRequest
*
* Revision 1.19  2002/09/03 16:00:50  juengel
* HeadControlMode follwTail, TailMode stayAsForced.
*
* Revision 1.18  2002/07/23 13:33:41  loetzsch
* new streaming classes
*
* removed many #include statements
*
* Revision 1.17  2002/07/07 09:43:27  roefer
* Memory leak closed
*
* Revision 1.16  2002/06/27 11:18:59  risler
* added additional walk type for bar challenge
*
* Revision 1.15  2002/06/22 04:35:50  dueffert
* 4 bar chall
*
* Revision 1.14  2002/06/21 04:53:38  risler
* added walktype walkWithBar
*
* Revision 1.13  2002/06/12 13:50:26  loetzsch
* changed #ifndef WIN32  to #ifndef _WIN32
*
* Revision 1.12  2002/06/12 11:26:28  dueffert
* if timestamp of last MotionRequest is too old, we will only swing on robot, not in RobotControl
*
* Revision 1.11  2002/06/10 11:05:17  risler
* added timestamp to motion request
* motioncontrol executes swing when no request was received
*
* Revision 1.10  2002/06/06 15:50:44  risler
* inverted tail tilt angle
*
* Revision 1.9  2002/06/06 12:51:44  risler
* removed bug with MotionRequest == operator / executed motion request
*
* Revision 1.8  2002/06/04 16:44:55  risler
* tailRequest added
*
* Revision 1.7  2002/06/01 13:38:35  risler
* reorganized walking engine selection
*
* Revision 1.6  2002/05/31 17:22:43  loetzsch
* added module selector for dribble walk type
*
* Revision 1.5  2002/05/16 22:36:11  roefer
* Team communication and GTMath bugs fixed
*
* Revision 1.4  2002/05/16 15:05:49  risler
* added walk type walkWithBall
*
* Revision 1.3  2002/05/15 08:08:53  risler
* changed default walk, added InvKin:Dortmund to selector
*
* Revision 1.2  2002/05/14 13:50:56  rentmeister
* DortmundWalkingParameters
*
* Revision 1.1.1.1  2002/05/10 12:40:15  cvsadm
* Moved GT2002 Project from ute to tamara.
*
* Revision 1.28  2002/05/06 15:57:43  risler
* corrected bodyRoll angle
*
* Revision 1.27  2002/05/05 15:14:23  risler
* changed stand implementation
*
* Revision 1.26  2002/05/04 18:23:43  risler
* added calculation of executedMotionRequest
*
* Revision 1.25  2002/05/04 12:43:38  loetzsch
* The currently executed MotionRequest is now sent from the MotionControl
* to the BehaviorControl via the OdometryData structure
*
* Revision 1.24  2002/04/29 13:48:24  risler
* added lastWalkType to WalkingEngine execute
*
* Revision 1.23  2002/04/26 13:35:33  risler
* DarmstadtGOWalkingEngine renamed to InvKinWalkingEngine
* added InvKinParameterSets
*
* Revision 1.22  2002/04/23 15:08:44  risler
* changed MotionRequest: walk instead of normalWalk,... and walkType added
*
* Revision 1.21  2002/04/23 10:38:31  risler
* renamed headOdometry to headState
*
* Revision 1.20  2002/04/16 16:59:47  dueffert
* newWalkingParameters added
*
* Revision 1.19  2002/04/15 13:39:24  rentmeister
* DoWalkingEngine (Dortmunder WalkingEngine)
*
* Revision 1.18  2002/04/08 09:48:54  rentmeister
* galopWalk für den Evolver
*
* Revision 1.17  2002/04/05 14:08:43  jhoffman
* stabilizer stuff
*
* Revision 1.16  2002/04/04 15:14:49  jhoffman
* added motion stabilizer
*
* Revision 1.15  2002/04/04 15:07:59  rentmeister
* Walk with Ball hinzugefügt
*
* Revision 1.14  2002/04/03 16:44:31  jhoffman
* added "stabilizeRobot" to motionControl (which is turned off as a default)
*
* Revision 1.13  2002/02/28 16:28:25  risler
* added GT2001WalkingParameters
*
* Revision 1.12  2002/02/23 16:33:07  risler
* finished GetupEngine
*
* Revision 1.11  2002/02/20 16:40:50  risler
* added GetupEngine
*
* Revision 1.10  2002/02/08 20:00:01  risler
* added DebugMotionControl
*
* Revision 1.9  2002/02/08 17:48:57  risler
* SensorData to MotionControl
*
* Revision 1.8  2002/01/19 12:42:32  risler
* fixed bug with solutionHandler
*
* Revision 1.7  2001/12/10 17:47:06  risler
* change log added
*
*/

---