Modules/MotionControl/GT2005MotionControl.cpp

Go to the documentation of this file.

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

#include "GT2005MotionControl.h"

#include "Modules/WalkingEngine/WalkingEngineSelector.h"
#include "Modules/WalkingEngine/InvKinWalkingEngine.h"
#include "Modules/WalkingEngine/InvKinWalkingParameterSets.h"
#include "Modules/SpecialActions/GT2003MotionNetSpecialActions/GT2003MotionNetSpecialActions.h"
#include "Platform/GTAssert.h"
#include "Platform/SystemCall.h"
#include "Tools/Actorics/Kinematics.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/RobotConfiguration.h"

GT2005MotionControl::GT2005MotionControl(ModuleHandler& moduleHandler,
                                           const MotionControlInterfaces& interfaces) 
                                           :MotionControl(interfaces),
                                           moduleHandler(moduleHandler),
                                           changeOfMotionControlStatePossible(true),
                                           lastMotionType(MotionRequest::playDead),
                                           motionControlState(GT2005MotionControl::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, gt2005Parameters, gt2005DebugData); //****for GT2005WalkingEngine****

  // create the InvKinWalking engine as only one instance is created
  // and used by different walk type with different parameter sets
  pInvKinWalkingEngine = new InvKinWalkingEngine(walkingEngineInterfaces);
  
  WalkingEngineSelector* tmp = new WalkingEngineSelector(moduleHandler, SolutionRequest::walkingEngineNormal,
    walkingEngineInterfaces, pInvKinWalkingEngine);
  pWalkingEngine[WalkRequest::normal] = tmp;
  
  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] = tmp;
  pWalkingEngine[WalkRequest::debug] = 
    new ParamInvKinWalkingEngine(new ERS7EvolveWalkingParameters, pInvKinWalkingEngine);
  
  pWalkingEngine[WalkRequest::boost] = tmp;

  pWalkingEngine[WalkRequest::walkWithBall] = tmp;

  pWalkingEngine[WalkRequest::gt2004mode] = 
    new GT2004WalkingEngine(walkingEngineInterfaces);

  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;
}

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

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

void GT2005MotionControl::setMotionControlState()
{
  switch(motionRequest.motionType)
  {
  case MotionRequest::playDead:
    motionControlState = GT2005MotionControl::playDead;
    return;
  case MotionRequest::getup:
    motionControlState = GT2005MotionControl::getup;
    return;
  case MotionRequest::stand:
    motionControlState = GT2005MotionControl::stand;
    return;
  case MotionRequest::specialAction:
    motionControlState = GT2005MotionControl::specialAction;
    return;
  case MotionRequest::walk:
    motionControlState = GT2005MotionControl::walk;
    return;
  }
}
void GT2005MotionControl::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 GT2005MotionControl::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 GT2005MotionControl::wakeUp:
      {
        changeOfMotionControlStatePossible
          = !wakeUpEngine.execute(frameNumber, sensorDataBuffer.frame[0], currentFrame, pidData);
        lastMotionType = MotionRequest::playDead;
        break;
      }
      case GT2005MotionControl::getup:
        {
          changeOfMotionControlStatePossible = !pGetupEngine->executeParameterized(currentFrame);
          lastMotionType = MotionRequest::getup;
          break;
        }
    case GT2005MotionControl::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 GT2005MotionControl::specialAction:
      changeOfMotionControlStatePossible 
        = !pSpecialActions->executeParameterized(latestSpecialActionRequest, currentFrame);
      lastMotionType = MotionRequest::specialAction;
      break;
      
    case GT2005MotionControl::walk:
      {
        changeOfMotionControlStatePossible 
          = !pWalkingEngine[latestWalkRequest.walkType]->executeParameterized(currentFrame, latestWalkRequest, positionInWalkCycle);
        positionInWalkCycle = motionInfo.positionInWalkCycle;
        lastMotionType = MotionRequest::walk;
        break;
      }
    }

    if(motionControlState != GT2005MotionControl::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:*/
    DEBUG_RESPONSE("LED, tail, mouth:tail shows frameloss",
      wagTail(motionRequest, 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 GT2005MotionControl::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 GT2005MotionControl::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);
  case idGT2005Request:
    return pWalkingEngine[WalkRequest::normal]->handleMessage(message);
  }
  return false;
}

---