Homepage Demos Overview Downloads Tutorials Reference
Credits

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

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:39 2004 by Doxygen 1.3.9.1