Modules/SpecialActions/GT2003MotionNetSpecialActions/GT2003MotionNetSpecialActions.cpp

Go to the documentation of this file.

/**
* @file GT2003MotionNetSpecialActions.cpp
* 
* This file contains a class for GT2003 motion net special actions.
* @author Uwe Düffert, Max Risler
*/

#include "GT2003MotionNetSpecialActions.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Debugging/Debugging.h"

GT2003MotionNetSpecialActions::GT2003MotionNetSpecialActions
(SpecialActionsInterfaces& interfaces)
: SpecialActions(interfaces),lastDataValid(false),dataRepetitionCounter(0),
  lastSpecialActionType(SpecialActionRequest::numOfSpecialAction)
{
  InConfigFile* file = new InConfigFile("spec_act.dat");

  motionNetData.load(*file);

  delete file;

  readOdometryTable();
  
  // create an uninitialised motion request to set startup motion
  SpecialActionRequest mr;
  currentNode=motionNetData.label_extern_start[(int)mr.specialActionType];
}

void GT2003MotionNetSpecialActions::readOdometryTable()
{
  char fileEntry[256];
  double tmp;
  int i,t;
  char leaveWalkingEngineCycle[1000];
  
  // initialise executedMotionRequest entries
  for (i=0; i<SpecialActionRequest::numOfSpecialAction; i++) 
  {
    executedMotionRequestTable[i].motionType = MotionRequest::specialAction;
    executedMotionRequestTable[i].specialActionRequest.specialActionType = (SpecialActionRequest::SpecialActionID)i;
  }
  //entry for extern
  executedMotionRequestTable[i].motionType = MotionRequest::specialAction;
  executedMotionRequestTable[i].specialActionRequest.specialActionType = (SpecialActionRequest::SpecialActionID)0;
  
  // read entries from file
  InConfigFile odometryFile("Odometry.cfg","GT2003MotionNetSpecialActions");
  if (!odometryFile.exists() || odometryFile.eof()) {
    OUTPUT(idText,text,"GT2003MotionNetSpecialActions : Error, odometry not found.");
  } else {
    while(!odometryFile.eof()) {
      odometryFile >> fileEntry;
      if (fileEntry[0] == '[') break; //next section
      if (strlen(fileEntry) > 0) {
        for (i=0; i<SpecialActionRequest::numOfSpecialAction; i++) {
          if (strcmp(fileEntry,SpecialActionRequest::getSpecialActionIDName((SpecialActionRequest::SpecialActionID)i))==0) {
            odometryFile >> t;
            switch(t) {
            case 0:
              // no odometry
              odometryTable[i].type = OdometryEntry::none;
              break;
            case 1:
              // once
              odometryTable[i].type = OdometryEntry::once;
              odometryFile >> odometryTable[i].o.translation.x;
              odometryFile >> odometryTable[i].o.translation.y;
              odometryFile >> tmp;
              odometryTable[i].o.fromAngle(tmp);
              break;
            case 2:
              // homogeneous
              odometryTable[i].type = OdometryEntry::homogeneous;
              odometryFile >> odometryTable[i].o.translation.x;
              odometryFile >> odometryTable[i].o.translation.y;
              odometryFile >> tmp;
              odometryTable[i].o.fromAngle(tmp);
              // convert from mm/seconds to mm/tick
              double motionCycleTime = getRobotConfiguration().getRobotDimensions().motionCycleTime;
              odometryTable[i].o.translation.x *= motionCycleTime;
              odometryTable[i].o.translation.y *= motionCycleTime;
              // convert from rad/seconds to rad/tick
              odometryTable[i].o.rotation *= motionCycleTime;
              break;
            }
            odometryFile >> motionInfoTable[i].neckHeight;
            odometryFile >> t;
            motionInfoTable[i].motionIsStable = (t!=0);
            motionInfoTable[i].executedMotionRequest.motionType = MotionRequest::specialAction;
            motionInfoTable[i].executedMotionRequest.specialActionRequest.specialActionType = (SpecialActionRequest::SpecialActionID)i;
            // body-tilt invalid, get other tilt from bodyPosture
            motionInfoTable[i].bodyTilt = 0;
            odometryFile >> leaveWalkingEngineCycle;
            odometryTable[i].walkCycleDissection = strlen(leaveWalkingEngineCycle);
            {
              for(unsigned int j = 0; j < strlen(leaveWalkingEngineCycle); ++j)
              {
                if(leaveWalkingEngineCycle[j] == 'A')
                {
                  odometryTable[i].leaveWalkCycleSections.push_back(j);
                }
              }
            }
            break;
          }
        }
        if (i==SpecialActionRequest::numOfSpecialAction) {
          OUTPUT(idText,text,"GT2003MotionNetSpecialActions : Error, invalid odometry entry for :");
          OUTPUT(idText,text,fileEntry);
        }
      }
    }
  }
}

bool GT2003MotionNetSpecialActions::getNextData(const SpecialActionRequest& specialActionRequest)
{
  while((MotionNetNode::NodeType)motionNetData.nodeArray[currentNode].d[0] != MotionNetNode::typeData)
  {
    switch ((MotionNetNode::NodeType)motionNetData.nodeArray[currentNode].d[0])
    {
    case MotionNetNode::typePID:
      {
        // set pid values
        motionNetData.nodeArray[currentNode++].toPIDData(pidData);
        break;
      }
    case MotionNetNode::typeConditionalTransition:
      if ((motionRequest.motionType != MotionRequest::specialAction)||
        (motionNetData.nodeArray[currentNode].d[2] != (short) specialActionRequest.specialActionType))
      {
        currentNode++;
        break;
      }
      //no break here: if condition is true, continue with transition!
    case MotionNetNode::typeTransition:
      // follow transition
      if (currentNode==0)
      {
        //we come from extern
        if (motionRequest.motionType == MotionRequest::specialAction)
        {
          currentNode = motionNetData.label_extern_start[(short)specialActionRequest.specialActionType];
        }
      }
      else
      {
        currentNode = motionNetData.nodeArray[currentNode].d[1];
      }
      // leave if transition to external motion
      if (currentNode==0)
      {
        return false;
      }
      break;
    }
  }
  
  motionNetData.nodeArray[currentNode].toJointData(currentData, dataRepetitionLength, interpolationMode);
  dataRepetitionCounter = dataRepetitionLength;

  //get neckHeight and wanted MotionRequest fromMotionInfoTable:
  motionNetData.nodeArray[currentNode++].toExecutedMotionRequest(motionInfo.executedMotionRequest);
  motionInfo.bodyTilt = motionInfoTable[motionInfo.executedMotionRequest.specialActionRequest.specialActionType].bodyTilt;
  motionInfo.motionIsStable = motionInfoTable[motionInfo.executedMotionRequest.specialActionRequest.specialActionType].motionIsStable;
  motionInfo.neckHeight = motionInfoTable[motionInfo.executedMotionRequest.specialActionRequest.specialActionType].neckHeight;
 
  //get currently executed MotionRequest from motion net traversal:
  if (
    motionInfo.executedMotionRequest.motionType == MotionRequest::specialAction &&
    motionInfo.executedMotionRequest.specialActionRequest.specialActionType != lastSpecialActionType)
  {
    currentOdometry = odometryTable[motionInfo.executedMotionRequest.specialActionRequest.specialActionType];
    lastSpecialActionType = motionInfo.executedMotionRequest.specialActionRequest.specialActionType;
  }
  
  return true;
}

bool GT2003MotionNetSpecialActions::handleMessage(InMessage& message)
{
  if (message.getMessageID() == idMotionNet)
  {
    motionNetData.load(message.config);
    return true;
  }
  return false;
}

void GT2003MotionNetSpecialActions::calculateJointData(JointData& jointData)
{
  if (interpolationMode && lastDataValid) {
    for (int i=0;i<JointData::numOfJoint;i++)
      if (lastData.data[i] == jointDataInvalidValue)
        jointData.data[i] = currentData.data[i];
      else if (currentData.data[i] == jointDataInvalidValue)
        jointData.data[i] = jointDataInvalidValue;
      else
        jointData.data[i] = currentData.data[i] +
        (lastData.data[i] - currentData.data[i]) * 
        dataRepetitionCounter / dataRepetitionLength;
  } else
    jointData = currentData;
}

bool GT2003MotionNetSpecialActions::executeParameterized(SpecialActionRequest specialActionRequest, JointData& jointData)
{
  if (dataRepetitionCounter <= 0)
  {
    if (lastMotionType != MotionRequest::specialAction)
    {
      //entered from external motion
      currentNode=0;
      lastDataValid = false;
      lastSpecialActionType = SpecialActionRequest::numOfSpecialAction;
    }
    // search next data, leave on transition to external motion
    if (!getNextData(specialActionRequest))
      return false;
  }
  else
    dataRepetitionCounter--;
  
  //set current joint values
  calculateJointData(jointData);
  
  //odometry update
  if (currentOdometry.type == OdometryEntry::homogeneous)
    odometryData.conc(currentOdometry.o);
  else if (currentOdometry.type == OdometryEntry::once) {
    odometryData.conc(currentOdometry.o);
    currentOdometry.type = OdometryEntry::none;
  }
  
  //store value if current data line finished
  if (dataRepetitionCounter <= 0) {
    lastData = currentData;
    lastDataValid = true;
  }
  return true;
}

bool GT2003MotionNetSpecialActions::specialActionIsExecutableInWalkingCycle(int specialActionID, double walkCycle){
  if(odometryTable[specialActionID].walkCycleDissection)
  {
    for(unsigned int i = 0; i < odometryTable[specialActionID].leaveWalkCycleSections.size();++i)
    {
      if((double)odometryTable[specialActionID].leaveWalkCycleSections[i]/(double)odometryTable[specialActionID].walkCycleDissection <= walkCycle 
        && (double)odometryTable[specialActionID].leaveWalkCycleSections[i]+(1.0/(double)odometryTable[specialActionID].walkCycleDissection) >= walkCycle)
        return true;
    }
    return false;
  }
  return true;
}

---