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