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

# Modules/SpecialActions/GT2003MotionNetSpecialActions/GT2003MotionNetSpecialActions.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2003MotionNetSpecialActions.cpp
    00003 *
    00004 * This file contains a class for GT2003 motion net special actions.
    00005 * @author Uwe Düffert, Max Risler
    00006 */
    00007
    00008 #include "GT2003MotionNetSpecialActions.h"
    00009 #include "Tools/Streams/InStreams.h"
    00010 #include "Tools/RobotConfiguration.h"
    00011 #include "Tools/Debugging/Debugging.h"
    00012
    00013 GT2003MotionNetSpecialActions::GT2003MotionNetSpecialActions
    00014 (SpecialActionsInterfaces& interfaces)
    00015 : SpecialActions(interfaces),lastDataValid(false),dataRepetitionCounter(0),
    00016   lastSpecialActionType(SpecialActionRequest::numOfSpecialAction)
    00017 {
    00018   InConfigFile* file = new InConfigFile("spec_act.dat");
    00019
    00020   motionNetData.load(*file);
    00021
    00022   delete file;
    00023
    00024   readOdometryTable();
    00025
    00026   // create an uninitialised motion request to set startup motion
    00027   SpecialActionRequest mr;
    00028   currentNode=motionNetData.label_extern_start[(int)mr.specialActionType];
    00029 }
    00030
    00031 void GT2003MotionNetSpecialActions::readOdometryTable()
    00032 {
    00033   char fileEntry[256];
    00034   double tmp;
    00035   int i,t;
    00036   char leaveWalkingEngineCycle[1000];
    00037
    00038   // initialise executedMotionRequest entries
    00039   for (i=0; i<SpecialActionRequest::numOfSpecialAction; i++)
    00040   {
    00041     executedMotionRequestTable[i].motionType = MotionRequest::specialAction;
    00042     executedMotionRequestTable[i].specialActionRequest.specialActionType = (SpecialActionRequest::SpecialActionID)i;
    00043   }
    00044   //entry for extern
    00045   executedMotionRequestTable[i].motionType = MotionRequest::specialAction;
    00046   executedMotionRequestTable[i].specialActionRequest.specialActionType = (SpecialActionRequest::SpecialActionID)0;
    00047
    00048   // read entries from file
    00049   InConfigFile odometryFile("Odometry.cfg","GT2003MotionNetSpecialActions");
    00050   if (!odometryFile.exists() || odometryFile.eof()) {
    00051     OUTPUT(idText,text,"GT2003MotionNetSpecialActions : Error, odometry not found.");
    00052   } else {
    00053     while(!odometryFile.eof()) {
    00054       odometryFile >> fileEntry;
    00055       if (fileEntry[0] == '[') break; //next section
    00056       if (strlen(fileEntry) > 0) {
    00057         for (i=0; i<SpecialActionRequest::numOfSpecialAction; i++) {
    00058           if (strcmp(fileEntry,SpecialActionRequest::getSpecialActionIDName((SpecialActionRequest::SpecialActionID)i))==0) {
    00059             odometryFile >> t;
    00060             switch(t) {
    00061             case 0:
    00062               // no odometry
    00063               odometryTable[i].type = OdometryEntry::none;
    00064               break;
    00065             case 1:
    00066               // once
    00067               odometryTable[i].type = OdometryEntry::once;
    00068               odometryFile >> odometryTable[i].o.translation.x;
    00069               odometryFile >> odometryTable[i].o.translation.y;
    00070               odometryFile >> tmp;
    00071               odometryTable[i].o.fromAngle(tmp);
    00072               break;
    00073             case 2:
    00074               // homogeneous
    00075               odometryTable[i].type = OdometryEntry::homogeneous;
    00076               odometryFile >> odometryTable[i].o.translation.x;
    00077               odometryFile >> odometryTable[i].o.translation.y;
    00078               odometryFile >> tmp;
    00079               odometryTable[i].o.fromAngle(tmp);
    00080               // convert from mm/seconds to mm/tick
    00081               double motionCycleTime = getRobotConfiguration().getRobotDimensions().motionCycleTime;
    00082               odometryTable[i].o.translation.x *= motionCycleTime;
    00083               odometryTable[i].o.translation.y *= motionCycleTime;
    00084               // convert from rad/seconds to rad/tick
    00085               odometryTable[i].o.rotation *= motionCycleTime;
    00086               break;
    00087             }
    00088             odometryFile >> motionInfoTable[i].neckHeight;
    00089             odometryFile >> t;
    00090             motionInfoTable[i].motionIsStable = (t!=0);
    00091             motionInfoTable[i].executedMotionRequest.motionType = MotionRequest::specialAction;
    00092             motionInfoTable[i].executedMotionRequest.specialActionRequest.specialActionType = (SpecialActionRequest::SpecialActionID)i;
    00093             // body-tilt invalid, get other tilt from bodyPosture
    00094             motionInfoTable[i].bodyTilt = 0;
    00095             odometryFile >> leaveWalkingEngineCycle;
    00096             odometryTable[i].walkCycleDissection = strlen(leaveWalkingEngineCycle);
    00097             {
    00098               for(unsigned int j = 0; j < strlen(leaveWalkingEngineCycle); ++j)
    00099               {
    00100                 if(leaveWalkingEngineCycle[j] == 'A')
    00101                 {
    00102                   odometryTable[i].leaveWalkCycleSections.push_back(j);
    00103                 }
    00104               }
    00105             }
    00106             break;
    00107           }
    00108         }
    00109         if (i==SpecialActionRequest::numOfSpecialAction) {
    00110           OUTPUT(idText,text,"GT2003MotionNetSpecialActions : Error, invalid odometry entry for :");
    00111           OUTPUT(idText,text,fileEntry);
    00112         }
    00113       }
    00114     }
    00115   }
    00116 }
    00117
    00118 bool GT2003MotionNetSpecialActions::getNextData(const SpecialActionRequest& specialActionRequest)
    00119 {
    00120   while((MotionNetNode::NodeType)motionNetData.nodeArray[currentNode].d[0] != MotionNetNode::typeData)
    00121   {
    00122     switch ((MotionNetNode::NodeType)motionNetData.nodeArray[currentNode].d[0])
    00123     {
    00124     case MotionNetNode::typePID:
    00125       {
    00126         // set pid values
    00127         motionNetData.nodeArray[currentNode++].toPIDData(pidData);
    00128         break;
    00129       }
    00130     case MotionNetNode::typeConditionalTransition:
    00131       if ((motionRequest.motionType != MotionRequest::specialAction)||
    00132         (motionNetData.nodeArray[currentNode].d[2] != (short) specialActionRequest.specialActionType))
    00133       {
    00134         currentNode++;
    00135         break;
    00136       }
    00137       //no break here: if condition is true, continue with transition!
    00138     case MotionNetNode::typeTransition:
    00139       // follow transition
    00140       if (currentNode==0)
    00141       {
    00142         //we come from extern
    00143         if (motionRequest.motionType == MotionRequest::specialAction)
    00144         {
    00145           currentNode = motionNetData.label_extern_start[(short)specialActionRequest.specialActionType];
    00146         }
    00147       }
    00148       else
    00149       {
    00150         currentNode = motionNetData.nodeArray[currentNode].d[1];
    00151       }
    00152       // leave if transition to external motion
    00153       if (currentNode==0)
    00154       {
    00155         return false;
    00156       }
    00157       break;
    00158     }
    00159   }
    00160
    00161   motionNetData.nodeArray[currentNode].toJointData(currentData, dataRepetitionLength, interpolationMode);
    00162   dataRepetitionCounter = dataRepetitionLength;
    00163
    00164   //get neckHeight and wanted MotionRequest fromMotionInfoTable:
    00165   motionNetData.nodeArray[currentNode++].toExecutedMotionRequest(motionInfo.executedMotionRequest);
    00166   motionInfo.bodyTilt = motionInfoTable[motionInfo.executedMotionRequest.specialActionRequest.specialActionType].bodyTilt;
    00167   motionInfo.motionIsStable = motionInfoTable[motionInfo.executedMotionRequest.specialActionRequest.specialActionType].motionIsStable;
    00168   motionInfo.neckHeight = motionInfoTable[motionInfo.executedMotionRequest.specialActionRequest.specialActionType].neckHeight;
    00169
    00170   //get currently executed MotionRequest from motion net traversal:
    00171   if (
    00172     motionInfo.executedMotionRequest.motionType == MotionRequest::specialAction &&
    00173     motionInfo.executedMotionRequest.specialActionRequest.specialActionType != lastSpecialActionType)
    00174   {
    00175     currentOdometry = odometryTable[motionInfo.executedMotionRequest.specialActionRequest.specialActionType];
    00176     lastSpecialActionType = motionInfo.executedMotionRequest.specialActionRequest.specialActionType;
    00177   }
    00178
    00179   return true;
    00180 }
    00181
    00182 bool GT2003MotionNetSpecialActions::handleMessage(InMessage& message)
    00183 {
    00184   if (message.getMessageID() == idMotionNet)
    00185   {
    00186     motionNetData.load(message.config);
    00187     return true;
    00188   }
    00189   return false;
    00190 }
    00191
    00192 void GT2003MotionNetSpecialActions::calculateJointData(JointData& jointData)
    00193 {
    00194   if (interpolationMode && lastDataValid) {
    00195     for (int i=0;i<JointData::numOfJoint;i++)
    00196       if (lastData.data[i] == jointDataInvalidValue)
    00197         jointData.data[i] = currentData.data[i];
    00198       else if (currentData.data[i] == jointDataInvalidValue)
    00199         jointData.data[i] = jointDataInvalidValue;
    00200       else
    00201         jointData.data[i] = currentData.data[i] +
    00202         (lastData.data[i] - currentData.data[i]) *
    00203         dataRepetitionCounter / dataRepetitionLength;
    00204   } else
    00205     jointData = currentData;
    00206 }
    00207
    00208 bool GT2003MotionNetSpecialActions::executeParameterized(SpecialActionRequest specialActionRequest, JointData& jointData)
    00209 {
    00210   if (dataRepetitionCounter <= 0)
    00211   {
    00212     if (lastMotionType != MotionRequest::specialAction)
    00213     {
    00214       //entered from external motion
    00215       currentNode=0;
    00216       lastDataValid = false;
    00217       lastSpecialActionType = SpecialActionRequest::numOfSpecialAction;
    00218     }
    00219     // search next data, leave on transition to external motion
    00220     if (!getNextData(specialActionRequest))
    00221       return false;
    00222   }
    00223   else
    00224     dataRepetitionCounter--;
    00225
    00226   //set current joint values
    00227   calculateJointData(jointData);
    00228
    00229   //odometry update
    00230   if (currentOdometry.type == OdometryEntry::homogeneous)
    00231     odometryData.conc(currentOdometry.o);
    00232   else if (currentOdometry.type == OdometryEntry::once) {
    00233     odometryData.conc(currentOdometry.o);
    00234     currentOdometry.type = OdometryEntry::none;
    00235   }
    00236
    00237   //store value if current data line finished
    00238   if (dataRepetitionCounter <= 0) {
    00239     lastData = currentData;
    00240     lastDataValid = true;
    00241   }
    00242   return true;
    00243 }
    00244
    00245 bool GT2003MotionNetSpecialActions::specialActionIsExecutableInWalkingCycle(int specialActionID, double walkCycle){
    00246   if(odometryTable[specialActionID].walkCycleDissection)
    00247   {
    00248     for(unsigned int i = 0; i < odometryTable[specialActionID].leaveWalkCycleSections.size();++i)
    00249     {
    00250       if((double)odometryTable[specialActionID].leaveWalkCycleSections[i]/(double)odometryTable[specialActionID].walkCycleDissection <= walkCycle
    00251         && (double)odometryTable[specialActionID].leaveWalkCycleSections[i]+(1.0/(double)odometryTable[specialActionID].walkCycleDissection) >= walkCycle)
    00252         return true;
    00253     }
    00254     return false;
    00255   }
    00256   return true;
    00257 }