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].o.walkCycleDissection = strlen(leaveWalkingEngineCycle);
            {
              for(unsigned int j = 0; j < strlen(leaveWalkingEngineCycle); ++j)
              {
                if(leaveWalkingEngineCycle[j] == 'A')
                {
                  odometryTable[i].o.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].o.walkCycleDissection)
  {
    for(unsigned int i = 0; i < odometryTable[specialActionID].o.leaveWalkCycleSections.size();++i)
    {
      if((double)odometryTable[specialActionID].o.leaveWalkCycleSections[i]/(double)odometryTable[specialActionID].o.walkCycleDissection <= walkCycle 
        && (double)odometryTable[specialActionID].o.leaveWalkCycleSections[i]+(1.0/(double)odometryTable[specialActionID].o.walkCycleDissection) >= walkCycle)
        return true;
    }
  }
  
  return false;
}



/*
* Change log :
* 
* $Log: GT2003MotionNetSpecialActions.cpp,v $
* Revision 1.11  2004/06/24 15:37:32  risler
* bugfix reset lastSpecialActionType so that odometry is set correctly when repeating the last special action
*
* Revision 1.10  2004/06/18 00:07:46  spranger
* bugfixxx
*
* Revision 1.9  2004/06/14 15:04:48  spranger
* changed interface of executeParametrized
* (this is not final, the specialActionRequest will be put into the interface)
*
* Revision 1.8  2004/06/09 11:29:51  thomas
* modified to remove warning
*
* Revision 1.7  2004/06/02 17:18:23  spranger
* MotionRequest cleanup
*
* Revision 1.6  2004/05/27 19:00:31  loetzsch
* bug fix
*
* Revision 1.5  2004/05/27 18:20:49  spranger
* motionControl cleaned up
*
* Revision 1.4  2004/05/27 09:29:28  loetzsch
* removed executedMotionRequest from Interfaces
*
* Revision 1.3  2004/05/26 17:37:30  dueffert
* comments fixed
*
* Revision 1.2  2004/05/26 17:21:47  dueffert
* better data types used
*
* Revision 1.1.1.1  2004/05/22 17:21:31  cvsadm
* created new repository GT2004_WM
*
* Revision 1.5  2004/03/20 09:55:25  roefer
* Preparation for improved odometry
*
* Revision 1.4  2004/01/03 16:36:13  roefer
* motionCycleTime 8 ms -> 0.008 s
*
* Revision 1.3  2004/01/01 10:58:50  roefer
* RobotDimensions are in a class now
*
* Revision 1.2  2003/12/16 19:01:18  loetzsch
* The motion net is not compiled into a C++ file but parsed at run time.
*
* Revision 1.1  2003/10/06 14:10:12  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.2  2003/09/26 11:38:52  juengel
* - sorted tools
* - clean-up in DataTypes
*
* 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.4  2003/06/21 14:55:06  risler
* odometry only once per special action
*
* Revision 1.3  2003/04/06 19:41:30  dueffert
* 2001 renamed to 2003
*
* Revision 1.2  2003/03/19 09:22:02  dueffert
* executedMotionRequest support added
*
* Revision 1.1  2003/03/10 14:14:08  dueffert
* optimized MotionNet
*
* Revision 1.6  2003/03/07 11:18:33  juengel
* headState.bodyRoll and headState.bodyTilt are not overwritten.
*
* Revision 1.5  2003/03/06 12:06:50  dueffert
* execute with parameters renamed to avoid inheritance warnings
*
* Revision 1.4  2002/11/20 18:02:29  risler
* added PID values to GT2001MotionNetSpecialActions
* added playDead motion
*
* Revision 1.3  2002/09/22 18:40:54  risler
* added new math functions, removed GTMath library
*
* Revision 1.2  2002/09/17 23:55:22  loetzsch
* - unraveled several datatypes
* - changed the WATCH macro
* - completed the process restructuring
*
* Revision 1.1  2002/09/10 15:36:16  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.4  2002/07/23 13:33:43  loetzsch
* new streaming classes
*
* removed many #include statements
*
* Revision 1.3  2002/06/28 10:26:19  roefer
* OUTPUT is possible in constructors
*
* Revision 1.2  2002/06/11 10:22:21  kallnik
* Pose2D neue Version (RotationVector) fertig
* PLEASE DON'T  READ ROTATION DIRECTLY USE getAngle()
*
* Revision 1.1.1.1  2002/05/10 12:40:16  cvsadm
* Moved GT2002 Project from ute to tamara.
*
* Revision 1.21  2002/05/06 15:26:44  loetzsch
* added several ";" after PRINT macros. (Didn't compile in Release version)
*
* Revision 1.20  2002/05/05 15:14:23  risler
* changed stand implementation
*
* Revision 1.19  2002/05/04 18:23:43  risler
* added calculation of executedMotionRequest
*
* Revision 1.18  2002/04/23 10:38:30  risler
* renamed headOdometry to headState
*
* Revision 1.17  2002/04/08 17:49:24  risler
* removed body tilt from odometry
*
* Revision 1.16  2002/04/02 14:01:39  dueffert
* minor odometry enhancements
*
* Revision 1.15  2002/04/02 13:10:20  dueffert
* big change: odometryData and cameraMatrix in image now, old logfiles may be obsolete
*
* Revision 1.14  2002/03/28 13:41:03  dueffert
* cast warning removed
*
* Revision 1.13  2002/03/27 15:54:28  risler
* added neckHeight and stableMotion in motion modules
*
* Revision 1.12  2002/02/13 01:41:24  risler
* no message
*
* Revision 1.11  2002/02/13 01:29:06  risler
* fixed crash on unsupported motion type
*
* Revision 1.10  2001/12/18 14:02:06  risler
* different odometry types in GT2001MotionNetSpecialActions
*
* Revision 1.9  2001/12/16 10:54:06  risler
* added InConfigFile
*
* Revision 1.8  2001/12/15 12:28:39  roefer
* SpecialActionsOdometry.cfg renamed to SAOdo.cfg
*
* Revision 1.7  2001/12/14 14:08:09  risler
* added odometry updates
*
* Revision 1.6  2001/12/13 19:13:58  risler
* added odometry updates in GT2001MotionNetSpecialActions
*
* Revision 1.5  2001/12/13 18:49:25  risler
* added odometry updates in GT2001MotionNetSpecialActions
*
* Revision 1.4  2001/12/12 12:43:33  risler
* removed MotionRequest.specialActionParams and MotionNetNode.pval
*
* Revision 1.3  2001/12/10 17:47:07  risler
* change log added
*
*/

---