Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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

Tekkotsu v1.4
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2