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 "Shared/mathutils.h"
00012 
00013 using namespace std; 
00014 
00015 void MotionExecThread::reset() {
00016   MarkScope sl(startLock);
00017   globals->setNextMotion(getNextMotion());
00018   if(globals->timeScale<=0 && isStarted())
00019     stop();
00020   else if(globals->timeScale>0 && !isStarted())
00021     start();
00022   else if(isStarted()) {
00023     interrupt();
00024   }
00025 }
00026 
00027 /*void MotionExecThread::start() {
00028   lastPoll=-1U;
00029   launching=initialPoll=true;
00030   Thread::start(); //skipping PollThread::start() because we don't want to reset startTime
00031 }*/
00032 
00033 bool MotionExecThread::poll() {
00034   MarkScope l(motionLock);
00035   PROFSECTION("ReadySendJoints()",*motionProfiler);
00036   //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)
00037   //ASSERTRETVAL(get_time()>=globals->getNextMotion()-1,"MotionExecThread::poll() early (time="<<get_time()<< " vs. nextMotion=" <<globals->getNextMotion()<<")",true);
00038   if(get_time()<globals->getNextMotion())
00039     return true;
00040   
00041   {
00042     MarkScope sensorLock(globals->sensorState);
00043     state->read(globals->sensorState,false);
00044   }
00045 
00046   NoCancelScope nc;
00047   
00048   if(globals->motion.verbose>=3)
00049     cout << "Motion processing at " << get_time() << endl;
00050   const unsigned int bufTime=NumFrames*FrameTime;
00051   unsigned int tgtSize=(globals->motion.feedbackDelay>0 ? (globals->motion.feedbackDelay)/bufTime+1 : 1);
00052   if(motionBuffers.size()<tgtSize) {
00053     motionBuffers.insert(motionBufferPos,new float[NumFrames][NumOutputs]);
00054     motionBufferPos--;
00055   } else {
00056     while(motionBuffers.size()>tgtSize) {
00057       std::list<float(*)[NumOutputs]>::iterator tmp=motionBufferPos++;
00058       delete [] *tmp;
00059       motionBuffers.erase(tmp);
00060       if(motionBufferPos==motionBuffers.end())
00061         motionBufferPos=motionBuffers.begin();
00062     }
00063   }
00064   if(lastPoll!=-1U && motionBuffers.size()>1) {
00065     std::list<float(*)[NumOutputs]>::iterator prev=motionBufferPos;
00066     if(prev==motionBuffers.begin())
00067       prev=motionBuffers.end();
00068     prev--;
00069     unsigned int cnt=motionBuffers.size()-1;
00070     while(get_time()>=lastPoll+bufTime*2) {
00071       lastPoll+=bufTime;
00072       if(globals->motion.verbose>=2)
00073         cout << "Dropped motion frame in poll() (late call)" << endl;
00074       if(cnt==0)
00075         continue; //we've overwritten all of the buffer, no need wrap through it again
00076       memcpy(*motionBufferPos,*prev,sizeof(float[NumFrames][NumOutputs]));
00077       prev=motionBufferPos;
00078       if(++motionBufferPos==motionBuffers.end())
00079         motionBufferPos=motionBuffers.begin();
00080       cnt--;
00081     }
00082   }
00083   try {
00084     motman->getOutputs(*motionBufferPos);
00085   } catch(const std::exception& ex) {
00086     if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManager processing",&ex))
00087       throw;
00088   } catch(...) {
00089     if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManager processing",NULL))
00090       throw;
00091   }
00092   Simulator::updateMotion(*motionBufferPos);
00093   if(++motionBufferPos==motionBuffers.end())
00094     motionBufferPos=motionBuffers.begin();
00095   {
00096     std::vector<std::pair<unsigned int, float[3]> > pids;
00097     try {
00098       motman->updatePIDs(pids);
00099     } catch(const std::exception& ex) {
00100       if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManager processing",&ex))
00101         throw;
00102     } catch(...) {
00103       if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManager processing",NULL))
00104         throw;
00105     }
00106     if(pids.size()>0) {
00107       // update MotionHooks / Drivers
00108       std::vector<MotionHook::PIDUpdate> updates;
00109       updates.reserve(pids.size());
00110       std::copy(pids.begin(),pids.end(),std::back_inserter(updates));
00111       Simulator::updatePIDs(updates);
00112       
00113       // copy over feedback values
00114       MarkScope sslock(globals->sensorState);
00115       for(std::vector<MotionHook::PIDUpdate>::const_iterator it=updates.begin(); it!=updates.end(); ++it)
00116         memcpy(globals->sensorState.pids[it->idx], it->pids, sizeof(float)*3);
00117     }
00118   }
00119   globals->setNextMotion(getNextMotion());
00120   lastPoll=get_time();
00121   applyPostureFeedback();
00122   return true;
00123 }
00124 
00125 bool MotionExecThread::launched() {
00126   if(globals->timeScale<=0)
00127     return false;
00128   startTime.Set();
00129   globals->setNextMotion(getNextMotion());
00130   //reset startTime to last motion time
00131   startTime-=(get_time()-(getNextMotion()-FrameTime*NumFrames))/globals->timeScale/1000;
00132   interrupted();
00133   //delay=(getNextMotion()>get_time()) ? (getNextMotion()-get_time())/globals->timeScale/1000 : 0;
00134   return PollThread::launched();
00135 }
00136 
00137 /*void MotionExecThread::cancelled() {
00138   bool isFullSpeed=(globals->timeScale<0);
00139 }*/
00140 
00141 void MotionExecThread::interrupted() {
00142   period=FrameTime*NumFrames/globals->timeScale/1000;
00143   delay=(globals->getNextMotion()-get_time())/globals->timeScale/1000+startTime.Age();
00144   //cout << "interrupt " << get_time() << ' ' << globals->getNextMotion() << ' ' << startTime.Age() << ' ' << delay << ' ' << period << ' ' << isStarted() << ' ' << globals->timeScale << endl;
00145 }
00146 
00147 void MotionExecThread::applyPostureFeedback() {
00148   if(globals->motion.feedbackDelay<0)
00149     return ;
00150   if(!globals->motion.override && !globals->sensorState.hasUnprovidedOutput())
00151     return;
00152   
00153   const unsigned int bufTime=NumFrames*FrameTime;
00154   MarkScope l(motionLock);
00155   if(lastPoll!=-1U && motionBuffers.size()>1) {
00156     std::list<float(*)[NumOutputs]>::iterator prev=motionBufferPos;
00157     if(prev==motionBuffers.begin())
00158       prev=motionBuffers.end();
00159     prev--;
00160     unsigned int cnt=motionBuffers.size()-1;
00161     while(get_time()>=lastPoll+bufTime*2) {
00162       lastPoll+=bufTime;
00163       cout << "Dropped motion frame in applyPostureFeedback()" << endl;
00164       if(cnt==0)
00165         continue; //we've overwritten all of the buffer, no need wrap through it again
00166       memcpy(*motionBufferPos,*prev,sizeof(float[NumFrames][NumOutputs]));
00167       prev=motionBufferPos;
00168       if(++motionBufferPos==motionBuffers.end())
00169         motionBufferPos=motionBuffers.begin();
00170       cnt--;
00171     }
00172   }
00173   unsigned int bufDelay=globals->motion.feedbackDelay/bufTime;
00174   unsigned int frameDelay=(globals->motion.feedbackDelay-bufDelay*bufTime)/FrameTime;
00175   int bufAdvance=motionBuffers.size()-1-bufDelay;
00176   std::list<float(*)[NumOutputs]>::iterator tgt=motionBufferPos;
00177   if(bufAdvance<0) {
00178     //cout << "negative bufAdvance " << bufAdvance << ' ' << globals->motion.feedbackDelay << ' ' << motionBuffers.size() << endl;
00179     frameDelay=0;
00180   } else if(bufAdvance>0) {
00181     //cout << "positive bufAdvance " << bufAdvance << ' ' << globals->motion.feedbackDelay << ' ' << motionBuffers.size() << endl;
00182     while(bufAdvance-- > 0) {
00183       if(++tgt==motionBuffers.end())
00184         tgt=motionBuffers.begin();
00185     }
00186   }
00187   float * outputs = (*tgt)[NumFrames-1-frameDelay];
00188   MarkScope sslock(globals->sensorState);
00189   for(unsigned int i=0; i<NumOutputs; ++i) {
00190     if(globals->motion.override || globals->sensorState.providedOutputs[i]==0) {
00191       float v = outputs[i];
00192       if(globals->motion.feedbackRangeLimits)
00193         v = mathutils::limitRange<float>(v, outputRanges[i][MinRange], outputRanges[i][MaxRange]);
00194       globals->sensorState.outputs[i] = v;
00195     }
00196   }
00197 }
00198 
00199 
00200 /*! @file
00201  * @brief 
00202  * @author Ethan Tira-Thompson (ejt) (Creator)
00203  */

Tekkotsu Hardware Abstraction Layer 5.1CVS
Generated Mon May 9 05:01:39 2016 by Doxygen 1.6.3