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
00027
00028
00029
00030
00031
00032 bool MotionExecThread::poll() {
00033 MarkScope l(motionLock);
00034 PROFSECTION("ReadySendJoints()",*motionProfiler);
00035
00036
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;
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
00084 cerr << "WARNING: Motion received NULL state for PID update on frame " << wsw.frame << " (now " << globals->motion.frameNumber << ")" << endl;
00085
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
00120 startTime-=(get_time()-(getNextMotion()-FrameTime*NumFrames))/globals->timeScale/1000;
00121 interrupted();
00122
00123 return PollThread::launched();
00124 }
00125
00126
00127
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
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;
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
00166 frameDelay=0;
00167 } else if(bufAdvance>0) {
00168
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
00184
00185
00186
00187
00188
00189
00190
00191
00192