Homepage Demos Overview Downloads Tutorials Reference
Credits

MotionSequenceEngine.cc

Go to the documentation of this file.
00001 #include "MotionSequenceMC.h"
00002 #include "Shared/get_time.h"
00003 #include "Shared/WorldState.h"
00004 #include "Shared/Config.h"
00005 #include <iostream>
00006 
00007 using std::cout;
00008 using std::endl;
00009 
00010 MotionSequenceEngine::Move_idx_t MotionSequenceEngine::invalid_move=-1U;
00011 
00012 int MotionSequenceEngine::updateOutputs() {
00013   if(isPlaying()) {
00014     if(lasttime==0)
00015       play();
00016     double diff=(get_time()-lasttime)*playspeed;
00017     if(diff+playtime<0)
00018       setTime(0);
00019     else
00020       setTime(static_cast<unsigned int>(diff+playtime));
00021     lasttime=get_time();
00022     return 1;
00023   } else {
00024     lasttime=get_time();
00025     return 0;
00026   }
00027 }
00028 
00029 const OutputCmd& MotionSequenceEngine::getOutputCmd(unsigned int i) {
00030   if(curstamps[i]!=playtime) {
00031     if(nexts[i]!=invalid_move)
00032       calcOutput(curs[i],playtime,getKeyFrame(prevs[i]),getKeyFrame(nexts[i]));
00033     else
00034       curs[i].unset();
00035     curstamps[i]=playtime;
00036   }
00037   return curs[i];
00038 }
00039 
00040 unsigned int MotionSequenceEngine::getBinSize() const {
00041   char buf[128];
00042   unsigned int len=128;
00043   unsigned int used=strlen("#MSq\n");
00044   used+=snprintf(buf,len,isSaveRadians()?"radians\n":"degrees\n");
00045   unsigned int t=0;
00046   Move_idx_t tprevs[NumOutputs];
00047   Move_idx_t tnexts[NumOutputs];
00048   for(unsigned int i=0;i<NumOutputs;i++)
00049     tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next;
00050   while(t!=-1U) {
00051     for(unsigned int i=0; i<NumOutputs; i++) {
00052       if((t!=0 || getKeyFrame(tprevs[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) {
00053         if(getKeyFrame(tprevs[i]).cmd.weight==1) {
00054           if(t!=0)
00055             used+=snprintf(buf,len,"%s\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode);
00056         } else
00057           used+=snprintf(buf,len,"%s\t%g\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode,getKeyFrame(tprevs[i]).cmd.weight);
00058       }
00059     }
00060     unsigned int last=t;
00061     t=setNextFrameTime(tprevs,tnexts);
00062     if(t!=-1U)
00063       used+=snprintf(buf,len,"advanceTime\t%d\n",t-last);
00064   }
00065   used+=strlen("#END\n");
00066   return used+1;
00067 }
00068 
00069 unsigned int MotionSequenceEngine::LoadBuffer(const char buf[], unsigned int len) {
00070   unsigned int origlen=len;
00071   if(strncmp("#POS",buf,4)==0) {
00072     // allow inlined loading of posture files
00073     PostureEngine pose;
00074     unsigned int used=pose.LoadBuffer(buf,len);
00075     if(used!=0)
00076       setPose(pose);
00077     return used;
00078   }
00079   if(strncmp("#MSq",buf,4)!=0) {
00080     // we don't want to display an error here because we may be only testing file type,
00081     // so it's up to the caller to decide if it's necessarily an error if the file isn't
00082     // a motion sequence
00083     //cout << "ERROR MotionSequenceEngine load corrupted - expected #MSq header" << endl;
00084     return 0;
00085   }
00086   unsigned int linenum=1;
00087   unsigned int lastOutputIdx=0;
00088   while(len<=origlen && len>0) {
00089     int written;
00090     //printf("%d %.9s\n",linenum+1,buf);
00091     if(buf[0]=='\r') {
00092       buf++; len--;
00093       if(buf[0]=='\n') {
00094         buf++; len--;
00095       }
00096       linenum++;
00097       continue;
00098     }
00099     if(buf[0]=='\n') {
00100       buf++; len--;
00101       linenum++;
00102       continue;
00103     }
00104     if(buf[0]=='#') {
00105       if(strncmp("#END\n",buf,5)==0 || strncmp("#END\r",buf,5)==0) {
00106         return origlen-len+5;
00107       } else if(strncmp("#END\r\n",buf,6)==0) {
00108         return origlen-len+6;
00109       } else {
00110         while(len>0 && *buf!='\n' && *buf!='\r') {len--;buf++;}
00111         if(*buf=='\n') { //in case of \r\n
00112           buf++;
00113           len--;
00114         }
00115         linenum++;
00116         continue;
00117       }
00118     }
00119     written=-1;
00120     const unsigned int cmdlen=16, arglen=32;
00121     char command[cmdlen];
00122     char arg1[arglen];
00123     char arg2[arglen];
00124     written=readWord(buf,&buf[len],command,cmdlen);
00125     if(!ChkAdvance(written,&buf,&len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0;
00126     written=readWord(buf,&buf[len],arg1,arglen);
00127     if(written>0)
00128       if(!ChkAdvance(written,&buf,&len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0;
00129     written=readWord(buf,&buf[len],arg2,arglen);
00130     if(written!=0)
00131       if(!ChkAdvance(written,&buf,&len,"*** ERROR MotionSequenceEngine load corrupted - line %d\n",linenum)) return 0;
00132     for(;len>0 && *buf!='\n' && *buf!='\r';buf++,len--) {}
00133     if(*buf=='\n') { //in case of \r\n
00134       buf++;
00135       len--;
00136     }
00137 
00138     if(strcasecmp(command,"delay")==0 || strcasecmp(command,"advanceTime")==0) {
00139       char* used;
00140       int delay = strtol(arg1,&used,0);
00141       if(*used!='\0') {
00142         cout << "*** WARNING illegal delay argument: " << arg1 << " - line " << linenum << endl;
00143       } else {
00144         setTime(playtime+delay);
00145       }
00146     } else if(strcasecmp(command,"settime")==0) {
00147       char* used;
00148       int newtime = strtol(arg1,&used,0);
00149       if(*used!='\0') {
00150         cout << "*** WARNING illegal settime argument: " << arg1 << " - line " << linenum << endl;
00151       } else {
00152         setTime(newtime);
00153       }
00154     } else if(strcasecmp(command,"load")==0) {
00155       PostureEngine pose;
00156       std::string f;
00157       if(arg1[0]!='/')
00158         f="/ms/data/motion/";
00159       f+=arg1;
00160       if(pose.LoadFile(f.c_str())!=0) {
00161         setPose(pose);
00162       } else
00163         cout << "*** WARNING could not read file " << arg1 << " for load - line " << linenum << endl;
00164     } else if(strcasecmp(command,"overlay")==0) {
00165       PostureEngine pose;
00166       std::string f;
00167       if(arg1[0]!='/')
00168         f="/ms/data/motion/";
00169       f+=arg1;
00170       if(pose.LoadFile(f.c_str())!=0)
00171         overlayPose(pose);
00172       else if(LoadFile(f.c_str())==0)
00173         cout << "*** WARNING could not read file " << arg1 << " for overlay - line " << linenum << endl;
00174     } else if(strcasecmp(command,"degrees")==0) {
00175       setSaveDegrees();
00176     } else if(strcasecmp(command,"radians")==0) {
00177       setSaveRadians();
00178     } else {
00179       lastOutputIdx=getOutputIndex(command,lastOutputIdx+1);
00180       if(lastOutputIdx==NumOutputs)
00181         cout << "*** WARNING " << command << " is not a valid joint on this model." << endl;
00182       else {
00183         char* used;
00184         double value=strtod(arg1,&used), weight=1;
00185         if(*used!='\0')
00186           cout << "*** WARNING illegal value argument: " << arg1 << " - line " << linenum << endl;
00187         else {
00188           if(arg2[0]!='\0') {
00189             weight=strtod(arg2,&used);
00190             if(*used!='\0') {
00191               cout << "*** WARNING illegal weight argument: " << arg2 << " - line " << linenum << endl;
00192               weight=1;
00193             }
00194           }
00195           setOutputCmd(lastOutputIdx,OutputCmd(value*loadSaveMode,weight));
00196         }
00197       }
00198     }
00199 
00200     linenum++;
00201 
00202   }
00203   cout << "*** WARNING MotionSequenceEngine load missing #END" << endl;
00204   return origlen-len;
00205 }
00206 
00207 unsigned int MotionSequenceEngine::SaveBuffer(char buf[], unsigned int len) const {
00208   //std::cout << "SAVEBUFFER..." << std::flush;
00209   unsigned int origlen=len;
00210   int written=snprintf(buf,len,"#MSq\n");
00211   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed on header\n")) return 0; if(len==0 || len>origlen) {
00212     cout << "*** ERROR MotionSequenceEngine save overflow on header" << endl;
00213     return 0;
00214   }
00215   written=snprintf(buf,len,isSaveRadians()?"radians\n":"degrees\n");
00216   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed on mode\n")) return 0; if(len==0 || len>origlen) {
00217     cout << "*** ERROR MotionSequenceEngine save overflow" << endl;
00218     return 0;
00219   }
00220   unsigned int t=0;
00221   Move_idx_t tprevs[NumOutputs];
00222   Move_idx_t tnexts[NumOutputs];
00223   for(unsigned int i=0;i<NumOutputs;i++)
00224     tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next;
00225   while(t!=-1U) {
00226 //    std::cout << "t="<<t<<"..."<<std::flush;
00227     for(unsigned int i=0; i<NumOutputs; i++) {
00228 //      cout << i << ' ' << outputNames[i] << ' ' << getKeyFrame(tprevs[i]).cmd.value << ' ' << getKeyFrame(tprevs[i]).cmd.weight << ' ' << getKeyFrame(tprevs[i]).starttime << " state: " << starts[i] <<' '<< prevs[i] <<' '<< nexts[i] << " cur: " << getKeyFrame(tprevs[i]).prev << ' ' << tprevs[i] << ' ' << getKeyFrame(tprevs[i]).next << endl;
00229       //first conditional is to skip 0 weighted values in first frame
00230       if((t!=0 || getKeyFrame(tprevs[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) {
00231         if(getKeyFrame(tprevs[i]).cmd.weight==1) {
00232           if(t!=0) {
00233             written=snprintf(buf,len,"%s\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode);
00234             if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed\n")) return 0;
00235           }
00236         } else {
00237           written=snprintf(buf,len,"%s\t%g\t%g\n",outputNames[i],getKeyFrame(tprevs[i]).cmd.value/loadSaveMode,getKeyFrame(tprevs[i]).cmd.weight);
00238           if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed\n")) return 0;
00239         }
00240         if(len==0 || len>origlen) {
00241           cout << "*** ERROR MotionSequenceEngine save overflow" << endl;
00242           return 0;
00243         }
00244       }
00245     }
00246     unsigned int last=t;
00247     t=setNextFrameTime(tprevs,tnexts);
00248     if(t!=-1U) {
00249       written=snprintf(buf,len,"delay\t%d\n",t-last);
00250       if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed\n")) return 0;
00251       if(len==0 || len>origlen) {
00252         cout << "*** ERROR MotionSequenceEngine save overflow" << endl;
00253         return 0;
00254       }
00255     }
00256   }
00257   written=snprintf(buf,len,"#END\n");
00258   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR MotionSequenceEngine save failed on #END\n")) return 0;
00259   if(len==0 || len>origlen) {
00260     cout << "*** ERROR MotionSequenceEngine save overflow on #END" << endl;
00261     return 0;
00262   }
00263   return origlen-len;
00264   cout << "SAVE-done!" << endl;
00265 }
00266 
00267 unsigned int MotionSequenceEngine::LoadFile(const char filename[]) {
00268   return LoadSave::LoadFile(config->motion.makePath(filename).c_str());
00269 }
00270 unsigned int MotionSequenceEngine::SaveFile(const char filename[]) const {
00271   return LoadSave::SaveFile(config->motion.makePath(filename).c_str());
00272 }
00273 
00274 void MotionSequenceEngine::setTime(unsigned int x) {
00275   playtime=x;
00276   for(unsigned int i=0; i<NumOutputs; i++)
00277     setRange(x,prevs[i],nexts[i]);
00278 }
00279 
00280 void MotionSequenceEngine::setOutputCmd(unsigned int i, const OutputCmd& cmd) {
00281   Move& p_m=getKeyFrame(prevs[i]);
00282   if(playtime==p_m.starttime) { //edit current keyframe
00283     p_m.cmd=cmd;
00284   } else { //add new keyframe
00285     Move_idx_t x = newKeyFrame();
00286     if(x==invalid_move) //ran out of memory - newKeyFrame should report error, we can silently die
00287       return;
00288     Move& cur=getKeyFrame(x);
00289     Move& prev_m=getKeyFrame(prevs[i]); //it's important to refresh this - newKeyFrame may have invalidated p_m reference
00290     cur.cmd=cmd;
00291     cur.starttime=playtime;
00292     cur.prev=prevs[i];
00293     cur.next=nexts[i];
00294     prev_m.next=x;
00295     if(nexts[i]!=invalid_move) //in middle
00296       getKeyFrame(nexts[i]).prev=x;
00297     else { //at end
00298       if(playtime>endtime)
00299         endtime=playtime;
00300     }
00301     prevs[i]=x;
00302 //    cout << "set " << i << ' ' << outputNames[i] << ' ' << cur.cmd.value << ' ' << cur.cmd.weight << ' ' << cur.starttime << " state: " << starts[i] <<' '<< prevs[i] <<' '<< prev_m.next <<' '<< nexts[i] << " new: " << cur.prev << ' ' << x << ' ' << cur.next << endl;
00303   }
00304 }
00305 
00306 void MotionSequenceEngine::setPose(const PostureEngine& pose) {
00307   for(unsigned int i=0; i<NumOutputs; i++)
00308     setOutputCmd(i,pose.getOutputCmd(i));
00309 }
00310 
00311 void MotionSequenceEngine::overlayPose(const PostureEngine& pose) {
00312   for(unsigned int i=0; i<NumOutputs; i++)
00313     if(pose.getOutputCmd(i).weight>0)
00314       setOutputCmd(i,pose.getOutputCmd(i));
00315 }
00316 
00317 void MotionSequenceEngine::compress() {
00318   for(unsigned int i=0; i<NumOutputs; i++) {
00319     Move_idx_t prev=getKeyFrame(starts[i]).next;
00320     if(prev==(Move_idx_t)-1)
00321       break;
00322     Move_idx_t cur=getKeyFrame(prev).next;
00323     if(cur==(Move_idx_t)-1)
00324       break;
00325     Move_idx_t next=getKeyFrame(cur).next;
00326     while(next!=(Move_idx_t)-1) {
00327       OutputCmd tmp;
00328       Move& prev_m=getKeyFrame(prev);
00329       Move& cur_m=getKeyFrame(cur);
00330       Move& next_m=getKeyFrame(next);
00331       calcOutput(tmp,cur_m.starttime,prev_m,next_m);
00332       if(tmp==cur_m.cmd || tmp.weight==0 && cur_m.cmd.weight==0) {
00333         prev_m.next=next;
00334         next_m.prev=prev;
00335         eraseKeyFrame(cur);
00336       } else
00337         prev=cur;
00338       cur=next;
00339       next=next_m.next;
00340     }
00341   }
00342 }
00343 
00344 void MotionSequenceEngine::makeSafe(const float vels[NumOutputs], float margin) {
00345   float comps[NumOutputs];
00346   for(unsigned int i=0;i<NumOutputs;i++)
00347     comps[i]=vels[i]*margin;
00348   unsigned int t=0;
00349   Move_idx_t tprevs[NumOutputs];
00350   Move_idx_t tnexts[NumOutputs];
00351   for(unsigned int i=0;i<NumOutputs;i++)
00352     tnexts[i]=getKeyFrame(tprevs[i]=starts[i]).next;
00353   while(t!=-1U) {
00354     for(unsigned int i=0; i<NumOutputs; i++) {
00355       //second and third conditionals are to skip transitions between 0 weighted frames
00356       if(tnexts[i]!=(Move_idx_t)-1 && (getKeyFrame(tprevs[i]).cmd.weight!=0 || getKeyFrame(tnexts[i]).cmd.weight!=0) && getKeyFrame(tprevs[i]).starttime==t) {
00357         float dv=fabs(getKeyFrame(tprevs[i]).cmd.value-getKeyFrame(tnexts[i]).cmd.value);
00358         unsigned int dt=getKeyFrame(tnexts[i]).starttime-getKeyFrame(tprevs[i]).starttime;
00359         if(dv/dt>comps[i]) {
00360           unsigned int delay=(unsigned int)(dv/comps[i])-dt;
00361           for(unsigned int j=0; j<NumOutputs; j++)
00362             for(Move_idx_t c=tnexts[j]; c!=(Move_idx_t)-1; c=getKeyFrame(c).next)
00363               getKeyFrame(c).starttime+=delay;
00364         }
00365       }
00366     }
00367     t=setNextFrameTime(tprevs,tnexts);
00368   }
00369   
00370 } 
00371 
00372 bool MotionSequenceEngine::isPlaying() {
00373   return playing && ((playspeed>0) ? (playtime<=endtime) : (playtime>0)); 
00374 }
00375 
00376 
00377 void MotionSequenceEngine::play() {
00378   if(playspeed>0)
00379     setTime(0);
00380   else
00381     setTime(endtime);
00382   resume();
00383 }
00384 
00385 void MotionSequenceEngine::resume() {
00386   playing=true;
00387   lasttime=get_time();
00388   for(unsigned int i=0; i<NumOutputs; i++) {
00389     Move_idx_t cur=starts[i];
00390     while(cur!=(Move_idx_t)-1) {
00391       if(getKeyFrame(cur).cmd.weight!=0) {
00392         getKeyFrame(starts[i]).cmd.value=state->outputs[i];
00393         break;
00394       }
00395       cur=getKeyFrame(cur).next;
00396     }
00397   }
00398 }
00399 
00400 unsigned int MotionSequenceEngine::setNextFrameTime(Move_idx_t p[NumOutputs], Move_idx_t n[NumOutputs]) const {
00401   unsigned int ans=-1U;
00402   for(unsigned int i=0; i<NumOutputs; i++)
00403     if(n[i]!=invalid_move && getKeyFrame(n[i]).starttime<ans)
00404       ans=getKeyFrame(n[i]).starttime;
00405   if(ans!=-1U)
00406     for(unsigned int i=0; i<NumOutputs; i++)
00407       setRange(ans,p[i],n[i]);
00408   return ans;
00409 }
00410 
00411 unsigned int MotionSequenceEngine::readWord(const char buf[], const char * const bufend, char wrd[], const unsigned int wordlen) {
00412   const char* origbuf=buf;
00413   wrd[0]='\0';
00414   unsigned int i;
00415   //skip whitespace
00416   for(;buf<bufend && isspace(*buf) && *buf!='\n' && *buf!='\r';buf++) {}
00417   //store wrd
00418   for(i=0; buf<bufend && !isspace(*buf); buf++)
00419     if(i<wordlen-1)
00420       wrd[i++]=*buf;
00421   wrd[i]='\0';
00422   if(buf>=bufend)
00423     return -1U;
00424   return buf-origbuf;
00425 }
00426 
00427 unsigned int MotionSequenceEngine::getOutputIndex(const char name[], unsigned int idx) {
00428   if(idx<NumOutputs) {
00429     unsigned int startidx=idx;
00430     for(;idx<NumOutputs;idx++)
00431       if(strcmp(name,outputNames[idx])==0)
00432         return idx;
00433     for(idx=0;idx<startidx;idx++)
00434       if(strcmp(name,outputNames[idx])==0)
00435         return idx;
00436     return NumOutputs;
00437   } else {
00438     for(idx=0;idx<NumOutputs;idx++)
00439       if(strcmp(name,outputNames[idx])==0)
00440         return idx;
00441     return idx;
00442   }
00443 }
00444 
00445 /*! @deprecated, call setTime(unsigned int x) instead */
00446 void MotionSequenceEngine::setPlayTime(unsigned int x) {
00447   setTime(x);
00448 }
00449 
00450 /*! @deprecated, use getTime() */
00451 unsigned int MotionSequenceEngine::getPlayTime() const {
00452   return getTime();
00453 }
00454 
00455 /*! @deprecated, use setSpeed(float x) */
00456 void MotionSequenceEngine::setPlaySpeed(float x) {
00457   setSpeed(x);
00458 }
00459 
00460 /*! @deprecated, use getSpeed() */
00461 float MotionSequenceEngine::getPlaySpeed() const {
00462   return getSpeed();
00463 }
00464 
00465 /*! @file
00466  * @brief Implements MotionSequenceEngine, abstract code for smoothly transitioning between a sequence of postures
00467  * @author ejt (Creator)
00468  *
00469  * $Author: ejt $
00470  * $Name: tekkotsu-2_2_2 $
00471  * $Revision: 1.3 $
00472  * $State: Exp $
00473  * $Date: 2004/12/21 22:22:54 $
00474  */
00475 

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0