Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MotionExecThread.cc

Go to the documentation of this file.
00001 #include "MotionExecThread.h"
00002 #include "Simulator.h"
00003 #include "SharedGlobals.h"
00004 #include "Shared/ProjectInterface.h"
00005 #include "Shared/WorldState.h"
00006 #include "Shared/MarkScope.h"
00007 #include "Motion/MotionManager.h"
00008 #include "IPC/MessageQueue.h"
00009 #include "Shared/debuget.h"
00010 #include "Shared/Profiler.h"
00011 #include "local/EntryPoint.h"
00012 
00013 using namespace std; 
00014 
00015 void MotionExecThread::reset() {
00016   globals->setNextMotion(getNextMotion());
00017   if(globals->timeScale<=0 && isStarted())
00018     stop();
00019   else if(globals->timeScale>0 && !isStarted())
00020     start();
00021   else if(isStarted()) {
00022     interrupt();
00023   }
00024 }
00025 
00026 /*void MotionExecThread::start() {
00027   lastPoll=-1U;
00028   launching=initialPoll=true;
00029   Thread::start(); //skipping PollThread::start() because we don't want to reset startTime
00030 }*/
00031 
00032 bool MotionExecThread::poll() {
00033   MarkScope l(motionLock);
00034   PROFSECTION("ReadySendJoints()",*motionProfiler);
00035   //this is OK: (can happen if the thread gets behind and the last one finished late, still want to do the current one, which appears to come early)
00036   //ASSERTRETVAL(get_time()>=globals->getNextMotion()-1,"MotionExecThread::poll() early (time="<<get_time()<< " vs. nextMotion=" <<globals->getNextMotion()<<")",true);
00037   if(get_time()<globals->getNextMotion())
00038     return true;
00039   try {
00040     if(globals->motion.verbose>=3)
00041       cout << "Motion processing at " << get_time() << endl;
00042     const unsigned int bufTime=NumFrames*FrameTime;
00043     unsigned int tgtSize=(globals->motion.feedbackDelay>0 ? (globals->motion.feedbackDelay)/bufTime+1 : 1);
00044     if(motionBuffers.size()<tgtSize) {
00045       motionBuffers.insert(motionBufferPos,new float[NumFrames][NumOutputs]);
00046       motionBufferPos--;
00047     } else {
00048       while(motionBuffers.size()>tgtSize) {
00049         std::list<float(*)[NumOutputs]>::iterator tmp=motionBufferPos++;
00050         delete [] *tmp;
00051         motionBuffers.erase(tmp);
00052         if(motionBufferPos==motionBuffers.end())
00053           motionBufferPos=motionBuffers.begin();
00054       }
00055     }
00056     if(lastPoll!=-1U && motionBuffers.size()>1) {
00057       std::list<float(*)[NumOutputs]>::iterator prev=motionBufferPos;
00058       if(prev==motionBuffers.begin())
00059         prev=motionBuffers.end();
00060       prev--;
00061       unsigned int cnt=motionBuffers.size()-1;
00062       while(get_time()>=lastPoll+bufTime*2) {
00063         lastPoll+=bufTime;
00064         if(globals->motion.verbose>=2)
00065           cout << "Dropped motion frame in poll() (late call)" << endl;
00066         if(cnt==0)
00067           continue; //we've overwritten all of the buffer, no need wrap through it again
00068         memcpy(*motionBufferPos,*prev,sizeof(float[NumFrames][NumOutputs]));
00069         prev=motionBufferPos;
00070         if(++motionBufferPos==motionBuffers.end())
00071           motionBufferPos=motionBuffers.begin();
00072         cnt--;
00073       }
00074     }
00075     motman->getOutputs(*motionBufferPos);
00076     Simulator::updateMotion(*motionBufferPos);
00077     if(++motionBufferPos==motionBuffers.end())
00078       motionBufferPos=motionBuffers.begin();
00079     {
00080       EntryPoint::WorldStateWrite wsw(globals->motion.frameNumber==-1U?0:globals->motion.frameNumber);
00081       MarkScope lw(motionLock,wsw);
00082       if(state==NULL) {
00083         // This should not be possible -- we requested the most recent frame, it should be available
00084         cerr << "WARNING: Motion received NULL state for PID update on frame " << wsw.frame << " (now " << globals->motion.frameNumber << ")" << endl;
00085         //just in case, try again one time
00086         lw.getResource().releaseResource(wsw);
00087         wsw.frame=globals->motion.frameNumber;
00088         lw.getResource().useResource(wsw);
00089       }
00090       if(state==NULL) {
00091         cerr << "WARNING: Motion PID update retry failed, falling back to previous WorldState (tried " << wsw.frame << ", now " << globals->motion.frameNumber << ")" << endl;
00092         state=wsw.src;
00093       } 
00094       if(state==NULL)
00095         cerr << "ERROR: Fallback failed, Motion unable to update PID values for frame " << wsw.frame << endl;
00096       else {
00097         if(!wsw.getComplete())
00098           state=wsw.src;
00099         motman->updatePIDs();
00100       }
00101     }
00102     globals->setNextMotion(getNextMotion());
00103   } catch(const std::exception& ex) {
00104     if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManager processing",&ex))
00105       throw;
00106   } catch(...) {
00107     if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManager processing",NULL))
00108       throw;
00109   }
00110   lastPoll=get_time();
00111   return true;
00112 }
00113 
00114 bool MotionExecThread::launched() {
00115   if(globals->timeScale<=0)
00116     return false;
00117   startTime.Set();
00118   globals->setNextMotion(getNextMotion());
00119   //reset startTime to last motion time
00120   startTime-=(get_time()-(getNextMotion()-FrameTime*NumFrames))/globals->timeScale/1000;
00121   interrupted();
00122   //delay=(getNextMotion()>get_time()) ? (getNextMotion()-get_time())/globals->timeScale/1000 : 0;
00123   return PollThread::launched();
00124 }
00125 
00126 /*void MotionExecThread::cancelled() {
00127   bool isFullSpeed=(globals->timeScale<0);
00128 }*/
00129 
00130 void MotionExecThread::interrupted() {
00131   period=FrameTime*NumFrames/globals->timeScale/1000;
00132   delay=(globals->getNextMotion()-get_time())/globals->timeScale/1000+startTime.Age();
00133   //cout << "interrupt " << get_time() << ' ' << globals->getNextMotion() << ' ' << startTime.Age() << ' ' << delay << ' ' << period << ' ' << isStarted() << ' ' << globals->timeScale << endl;
00134 }
00135 
00136 const PostureEngine& MotionExecThread::getPostureFeedback() {
00137   if(globals->motion.feedbackDelay<0)
00138     return curPose;
00139   
00140   const unsigned int bufTime=NumFrames*FrameTime;
00141   MarkScope l(motionLock);
00142   if(lastPoll!=-1U && motionBuffers.size()>1) {
00143     std::list<float(*)[NumOutputs]>::iterator prev=motionBufferPos;
00144     if(prev==motionBuffers.begin())
00145       prev=motionBuffers.end();
00146     prev--;
00147     unsigned int cnt=motionBuffers.size()-1;
00148     while(get_time()>=lastPoll+bufTime*2) {
00149       lastPoll+=bufTime;
00150       cout << "Dropped motion frame in getPostureFeedback()" << endl;
00151       if(cnt==0)
00152         continue; //we've overwritten all of the buffer, no need wrap through it again
00153       memcpy(*motionBufferPos,*prev,sizeof(float[NumFrames][NumOutputs]));
00154       prev=motionBufferPos;
00155       if(++motionBufferPos==motionBuffers.end())
00156         motionBufferPos=motionBuffers.begin();
00157       cnt--;
00158     }
00159   }
00160   unsigned int bufDelay=globals->motion.feedbackDelay/bufTime;
00161   unsigned int frameDelay=(globals->motion.feedbackDelay-bufDelay*bufTime)/FrameTime;
00162   int bufAdvance=motionBuffers.size()-1-bufDelay;
00163   std::list<float(*)[NumOutputs]>::iterator tgt=motionBufferPos;
00164   if(bufAdvance<0) {
00165     //cout << "negative bufAdvance " << bufAdvance << ' ' << globals->motion.feedbackDelay << ' ' << motionBuffers.size() << endl;
00166     frameDelay=0;
00167   } else if(bufAdvance>0) {
00168     //cout << "positive bufAdvance " << bufAdvance << ' ' << globals->motion.feedbackDelay << ' ' << motionBuffers.size() << endl;
00169     while(bufAdvance-- > 0) {
00170       if(++tgt==motionBuffers.end())
00171         tgt=motionBuffers.begin();
00172     }
00173   }
00174   float * outputs = (*tgt)[NumFrames-1-frameDelay];
00175   for(unsigned int i=0; i<NumOutputs; ++i) {
00176     curPose(i).value = outputs[i];
00177     curPose(i).weight = (globals->motion.override || globals->motion.providedOutputs[i]==0) ? 1 : 0;
00178   }
00179   return curPose;
00180 }
00181 
00182 
00183 /*! @file
00184  * @brief 
00185  * @author Ethan Tira-Thompson (ejt) (Creator)
00186  *
00187  * $Author: ejt $
00188  * $Name: tekkotsu-4_0 $
00189  * $Revision: 1.3 $
00190  * $State: Exp $
00191  * $Date: 2007/10/12 16:55:05 $
00192  */

Tekkotsu Hardware Abstraction Layer 4.0
Generated Thu Nov 22 01:00:53 2007 by Doxygen 1.5.4