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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4