Homepage Demos Overview Downloads Tutorials Reference
Credits

MotionSequenceMC.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_MotionSequenceMC_h_
00003 #define INCLUDED_MotionSequenceMC_h_
00004 
00005 #include "MotionCommand.h"
00006 #include "MotionManager.h"
00007 #include "Shared/LoadSave.h"
00008 #include "Shared/ListMemBuf.h"
00009 #include "PostureEngine.h"
00010 
00011 #include <iostream>
00012 
00013 //! A handy little (or not so little) class for switching between a sequence of postures
00014 /*! Outputs are handled independently.  It's easy to add keyframes
00015  *  which modify all of the outputs, but since each output is tracked
00016  *  individually, OutputCmd's with 0 weight can be used to not affect
00017  *  other motions.  For instance, pan the head left to right while
00018  *  moving the right leg up and down several times, you won't have to
00019  *  specify the position of the head in its motion at each of the leg
00020  *  motion keyframes.
00021  *
00022  *  Be aware that the 0 time frame will be replaced on a call to
00023  *  play() with the current body posture.  However, this only applies
00024  *  to  outputs which have a non-zero weighted frame defined at some
00025  *  point.  The weights, of the 0 time frame will remain unchanged.  
00026  *  These weights are initially set to 0, so that it's
00027  *  possible to 'fade in' the first frame of the motion sequence from
00028  *  whereever the body happens to be (or already doing)
00029  *
00030  *  To fade out at the end, set a frame with 0 weight for everything.
00031  *  Otherwise it will simply die suddenly.  When a joint reaches its
00032  *  last keyframe, it will be set to 0 weight for all future
00033  *  updateOutputs() (unless of course the playhead is reset)
00034  *
00035  *  Currently, MotionSequence's are intended mainly for building, 
00036  *  not editing.  It's easy to add keyframes, but hard/impossible to
00037  *  delete them.
00038  *
00039  *  The MotionSequence base class is an abstract class so that you can
00040  *  create memory efficient motion sequences and simply refer to them
00041  *  by the common base class instead of having to worry about the
00042  *  actual size allocated in the template, MotionSequenceMC.
00043  *
00044  *  @see MotionSequence::SizeSmall, MotionSequence::SizeMedium, MotionSequence::SizeLarge, MotionSequence::SizeXLarge, 
00045  *  
00046  *  The file format used is as follows: ('<' and '>' are not meant literally)
00047  *  - First line: '<tt>#MSq</tt>'
00048  *  - Followed by any series of:\n
00049  *    - '<tt>delay </tt><i>time-delta</i>' - moves playhead forward, in milliseconds
00050  *    - '<tt>settime </tt><i>time</i>' - sets play time to specified value, in ms
00051  *    - '<i>outputname</i><tt> </tt><i>value</i><tt> </tt>[<i>weight</i>]' - sets the specified output to the value - assumes 1 for weight; you can view the list of valid joint names in the outputNames array within the RobotInfo extension namespace for your model.  (e.g. ERS210Info::outputNames[])
00052  *    - '<tt>load </tt><i>filename</i>' - file is a posture, sets position
00053  *    - '<tt>overlay </tt><i>filename</i>' - file can be a posture or another motion sequence
00054  *    - '<tt>degrees</tt>' - following <i>value</i>s will be interpreted as degrees [default]
00055  *    - '<tt>radians</tt>' - following <i>value</i>s will be interpreted as radians
00056  *    - '<tt>#</tt><i>comment</i>' - a comment line
00057  *  - Last line: '<tt>#END</tt>'
00058  *  
00059  *  After loading a motion sequence, the playtime is left at the end.
00060  *  This is to make it easy to append/overlay motion sequences.
00061  *  However, the playhead will be reset to the beginning on the first
00062  *  call to updateOutputs() if isPlaying() returns true.
00063  *
00064  *  You can also create a MotionSequence dynamically at run time:
00065  *  \code
00066  *  //This code sample will stand up, point the head forward and up 0.1 radians,
00067  *  //and then autoprune
00068  *
00069  *  //First declare the MotionSequence itself:
00070  *  SharedObject< MotionSequenceMC<MotionSequence::SizeSmall> > stand;
00071  *
00072  *  //Over the course of 700 milliseconds, go to standing posture:
00073  *  standSit->setPlayTime(700);
00074  *  standSit->setPose(PostureEngine("/ms/data/motion/stand.pos"));
00075  *
00076  *  //Then take another 700 milliseconds to straighten out the head:
00077  *  standSit->setPlayTime(700);
00078  *  //We'll set joints individually this time, instead of loading a posture file:
00079  *  standSit->setOutputCmd(HeadOffset+PanOffset,0);
00080  *  standSit->setOutputCmd(HeadOffset+RollOffset,0);
00081  *  standSit->setOutputCmd(HeadOffset+TiltOffset,0.1); //look up .1 radians
00082  *
00083  *  //Add to MotionManager:
00084  *  motman->addMotion(standSit);
00085  *  //Playback will now begin automatically, and region deallocated when done
00086  *  \endcode
00087  *  
00088  *  By default, #playing is true.  Thus, when you add a MotionSequence
00089  *  to the MotionManager, it will begin executing automatically.  If
00090  *  you do \e not want this behavior, simply call pause() before
00091  *  adding the sequence.
00092  *
00093  *  When the sequence reaches the end, isAlive() will return false.
00094  *  By default, the motion sequence will autoprune itself from the
00095  *  MotionManager.  However, you can either pass \c false to
00096  *  MotionManager::addMotion(), or call setAutoPrune(false).
00097  *  
00098  *  Lines beginning with '#' are ignored as comments.  Be aware if you
00099  *  load the file and then save it again, these comments will be lost.
00100  *  
00101  *  @see PostureEngine for information on the posture files
00102  */
00103 class MotionSequence : public MotionCommand, public LoadSave {
00104 public:
00105   //!constructor, will start playing immediately
00106   MotionSequence() : MotionCommand(), playtime(1), lasttime(0), endtime(0), playspeed(1.0), playing(true), loadSaveMode(M_PI/180) {}
00107   //!destructor
00108   virtual ~MotionSequence() {}
00109 
00110   //!To avoid code bloat if there are a large number of different sized MotionSequences, use these sizes where possible.
00111   //!@name Template Sizes
00112   static const unsigned int SizeTiny   = NumOutputs*2;  //!< Tiny, but enough to handle a transition into a full-body pose
00113   static const unsigned int SizeSmall  = NumOutputs*3;  //!< Small, but still big enough to handle most of the included MS's (2 full-body frames ~ around 1KB)
00114   static const unsigned int SizeMedium = NumOutputs*6;  //!< Medium (5 full body frames ~ est 4KB)
00115   static const unsigned int SizeLarge  = NumOutputs*11;  //!< Large (10 full body frames ~ est 8KB)
00116   static const unsigned int SizeXLarge = NumOutputs*26;  //!< eXtra Large (25 full body frames ~ est 16KB)
00117   //@}
00118 
00119   //!@name Inherited from MotionCommand
00120   virtual int updateOutputs();
00121   virtual int isDirty() { return isPlaying(); }
00122   virtual int isAlive() { return (playspeed>0) ? (playtime<=endtime) : (playtime>0); }
00123   //@}
00124   
00125   //!@name LoadSave related
00126   virtual unsigned int getBinSize() const; //!< inherited, returns the size used to save the sequence
00127   virtual unsigned int LoadBuffer(const char buf[], unsigned int len); //!< inherited, doesn't clear before loading - call clear yourself if you want to reset, otherwise it will overlay.  Leaves playtime at end of load.
00128   virtual unsigned int SaveBuffer(char buf[], unsigned int len) const; //!< inherited, saves the motion sequence - will save a flat file - doesn't remember references to other files which were loaded
00129   virtual unsigned int LoadFile(const char filename[]); //!< inherited, doesn't clear before loading - call clear yourself if you want to reset, otherwise it will overlay.  Leaves playtime at end of load.
00130   virtual unsigned int SaveFile(const char filename[]) const; //!< inherited, saves the motion sequence - will save a flat file - doesn't remember references to other files which were loaded
00131   void setSaveDegrees() { loadSaveMode=M_PI/180; }       //!< will store angles as degrees on future saves
00132   bool isSaveDegrees() const { return loadSaveMode!=1; } //!< returns true if will store angles as degrees on future saves
00133   void setSaveRadians() { loadSaveMode=1; }              //!< will store angles as radians on future saves
00134   bool isSaveRadians() const { return loadSaveMode==1; } //!< returns true if will store angles as degrees on future saves
00135   //@}
00136   
00137   //!@name Sequence Construction
00138   virtual void clear()=0; //!< empties out the sequence (constant time operation - faster than a series of pops)
00139   void setPlayTime(unsigned int x); //!< set the time for both playback and editing (in milliseconds)
00140   void setOutputCmd(unsigned int i, const OutputCmd& cmd); //!< will insert a keyframe for the given output, or change an existing one
00141   const OutputCmd& getOutputCmd(unsigned int i); //!< gets the value of output @a i at the playhead
00142   void setPose(const PostureEngine& pose); //!< calls setOutputCmd on each of the OutputCmds in @a pose
00143   void overlayPose(const PostureEngine& pose); //!< calls setOutputCmd on non-zero weighted OutputCmds in @a pose
00144   void compress(); //!< compresses the sequence by eliminating sequences of moves which are identical
00145   virtual unsigned int getMaxFrames() const=0; //!< returns the maximum number of key frames (Move's) which can be stored, determined by the instantiating MotionSequenceMC's template parameter
00146   virtual unsigned int getUsedFrames() const=0; //!< returns the number of used key frames (Move's) which have been stored by the instantiation MotionSequence subclass
00147   void makeSafe(const float vels[NumOutputs], float margin); //!< will insert time into the motion where needed to keep the joint velocities at or below the speeds given in @a vels * @a margin
00148   //@}
00149 
00150   //!@name Playback Control
00151   bool isPlaying() { return playing && isAlive(); }       //!< returns true if currently playing
00152   void play();                                            //!< restarts playback from beginning
00153   void pause() { playing=false; }                         //!< pauses playback until another call to play() or resume()
00154   void resume();                                          //!< begins playback from the current playtime
00155   unsigned int getPlayTime() const { return playtime; }   //!< returns the current position of the playback (in milliseconds), see setPlayTime()
00156   unsigned int getEndTime() const { return endtime; }     //!< returns the length of the motion sequence (in milliseconds)
00157   void setPlaySpeed(float x) { playspeed=x; }            //!< sets the playback speed (e.g. 1=regular, 0.5=half speed, -1=@b backwards)
00158   float getPlaySpeed() const { return playspeed; }       //!< returns the playback speed
00159   //@}
00160 
00161 protected:
00162   // TYPES:
00163   typedef unsigned short Move_idx_t; //!< type for indexes to move structures in #moves
00164   static Move_idx_t invalid_move; //!< used to mark the ends of the Move linked lists
00165 
00166   //! This struct holds all the information needed about a frame for a particular output
00167   struct Move {
00168     //!constructor
00169     Move() : cmd(), next(), prev(), starttime(0) {}
00170     OutputCmd cmd;           //!< the actual command to use
00171     Move_idx_t next;        //!< the next frame
00172     Move_idx_t prev;        //!< the previous frame
00173     unsigned int starttime; //!< the time (relative to first frame) this frame should be expressed at
00174   };
00175 
00176   // MEMBERS:
00177   Move_idx_t starts[NumOutputs]; //!< the beginning frame for each output animation
00178   Move_idx_t prevs[NumOutputs];  //!< the previous frame (the starttime for this frame will always be less than or equal to playtime)
00179   Move_idx_t nexts[NumOutputs];  //!< the upcoming frame (the starttime for this frame will always be greater than playtime)
00180   OutputCmd curs[NumOutputs];          //!< merely a cache of current values (if computed, see #curstamps)
00181   unsigned int curstamps[NumOutputs]; //!< timestamp of corresponding value in #curs
00182   unsigned int playtime;              //!< the current time of playback, 0 is start of sequence
00183   unsigned int lasttime;              //!< the time of the last update
00184   unsigned int endtime;               //!< max of #moves's Move::starttime's
00185   float playspeed;                    //!< multiplies the difference between current time and starttime, negative will cause play backwards
00186   bool playing;                       //!< true if playing, false if paused
00187   
00188   float loadSaveMode;                 //!< 1 to use radians, M_PI/180 for degrees during a save
00189 
00190   virtual Move& getKeyFrame(Move_idx_t x) =0;            //!< returns the Move struct corresponding to @a x in the subclass's actual data structure
00191   virtual const Move& getKeyFrame(Move_idx_t x) const=0; //!< returns the Move struct corresponding to @a x in the subclass's actual data structure
00192   virtual Move_idx_t newKeyFrame()=0;                    //!< causes subclass to create a new Move structure, returns its index
00193   virtual void eraseKeyFrame(Move_idx_t x)=0;            //!< causes subclass to mark the corresponding Move structure as free
00194 
00195   //!Does the actual calculation of position information.  Perhaps replace with a Bezier or spline or something?
00196   void calcOutput(OutputCmd& ans, unsigned int t, const Move& prev, const Move& next) const {
00197     float prevweight=(float)(next.starttime-t)/(float)(next.starttime-prev.starttime);
00198     ans.set(prev.cmd,next.cmd,prevweight);
00199   }
00200   
00201   //!Sets prev and next to the appropriate values for the given time and output index
00202   virtual void setRange(unsigned int t,Move_idx_t& prev, Move_idx_t& next) const=0;
00203 
00204   //!sets playtime to next time for which any output has a keyframe, -1 if none exists
00205   unsigned int setNextFrameTime(Move_idx_t p[NumOutputs], Move_idx_t n[NumOutputs]) const;
00206   
00207   //!reads a line from a file, parsing it into variables, returns ending position
00208   static unsigned int readWord(const char buf[], const char * const buflen, char word[], const unsigned int wordlen);
00209 
00210   //!returns the index for the output named in the string or NumOutputs if not found, begins search through RobotInfo::outputName's at index @a i
00211   static unsigned int getOutputIndex(const char name[], unsigned int i);
00212 };
00213 
00214 //! Instantiates MotionSequences - when you want to make a new MotionSequence, make one of these
00215 /*! Allows a (compile-time) variable amount of data storage through its template parameter.
00216  *  See MotionSequence for documentation on its members
00217  *  @see MotionSequence
00218  *  @see MotionSequence::SizeSmall, MotionSequence::SizeMedium, MotionSequence::SizeLarge, MotionSequence::SizeXLarge, 
00219  *  */
00220 template<unsigned int MAXMOVE>
00221 class MotionSequenceMC : public MotionSequence {
00222 public:
00223   //!constructor
00224   MotionSequenceMC()
00225     : MotionSequence(), moves()
00226   {
00227     clear();
00228   }
00229   //!constructor, loads from a file and then resets the playtime to beginning and begins to play
00230   explicit MotionSequenceMC(const char* filename)
00231     : MotionSequence(), moves()
00232   {
00233     clear();
00234     LoadFile(filename);
00235     setPlayTime(1);
00236   }
00237   //!destructor
00238   virtual ~MotionSequenceMC() {}
00239 
00240   
00241   /*struct logent {
00242     logent(unsigned int t, unsigned int p, unsigned int i, OutputCmd f[NumFrames])
00243       : time(t), play(p), index(i)
00244     {
00245       for(unsigned int j=0;j<NumFrames;j++)
00246         frames[j]=f[j];
00247     }
00248     unsigned int time,play,index;
00249     OutputCmd frames[NumFrames];
00250   };
00251 
00252   std::vector<logent> log;
00253 
00254   virtual int isAlive() {
00255     if(MotionSequenceMC::isAlive())
00256       return true;
00257     cout << "LOG:" << endl;
00258     for(unsigned int i=0; i<log.size(); i++) {
00259       cout << '#' << '\t' << log[i].time << '\n';
00260       for(unsigned int j=0; j<NumFrames; j++)
00261         cout << log[i].play+j*FrameTime << ' ' << log[i].frames[j].value << ' ' << log[i].frames[j].weight << ' ' << log[i].index << '\n';
00262     }
00263     cout << endl;
00264     log.clear();
00265     return false;
00266     }*/
00267 
00268   // I put this here because i want direct access to moves so it'll be faster
00269   virtual int updateOutputs() {
00270     MotionSequence::updateOutputs();
00271     if(!isPlaying()) {
00272       for(unsigned int i=0; i<NumOutputs; i++) //just copies getOutputCmd(i) across frames
00273         motman->setOutput(this,i,getOutputCmd(i));
00274     } else {
00275       for(unsigned int i=0; i<NumOutputs; i++) { //fill out the buffer of commands for smoother movement
00276         Move_idx_t prev=prevs[i],next=nexts[i];
00277         OutputCmd frames[NumFrames];
00278         frames[0]=getOutputCmd(i);
00279         for(unsigned int t=playtime+FrameTime,j=1;j<NumFrames;j++,t+=FrameTime) {
00280           setRange(t,prev,next);
00281           if(next!=invalid_move)
00282             calcOutput(frames[j],t,moves[prev],moves[next]);
00283           else
00284             frames[j].unset();
00285         }
00286         motman->setOutput(this,i,frames);
00287       }
00288     }
00289     return NumOutputs;
00290     //    if(i<NumLegJointss)
00291     //      log.push_back(logent(get_time(),playtime,i,frames));
00292   }
00293   
00294   virtual void clear() {
00295     moves.clear();
00296     for(unsigned int i=0; i<NumOutputs; i++) {
00297       prevs[i]=starts[i]=moves.new_back();
00298       moves.back().cmd.unset();
00299       moves.back().next=invalid_move;
00300       moves.back().prev=invalid_move;
00301       nexts[i]=invalid_move;
00302     }
00303     setPlayTime(1);
00304   }
00305   virtual unsigned int getMaxFrames() const { return moves.getMaxCapacity(); }
00306   virtual unsigned int getUsedFrames() const { return moves.size(); }
00307 
00308 protected:
00309   // TYPES:
00310   typedef ListMemBuf<Move,MAXMOVE,Move_idx_t> list_t; //!< shorthand for the ListMemBuf that stores all of the movement frames
00311 
00312   // MEMBERS:
00313   list_t moves;                       //!< stores all of the movement keyframes
00314 
00315   virtual Move& getKeyFrame(Move_idx_t x) { return moves[x]; }
00316   virtual const Move& getKeyFrame(Move_idx_t x) const { return moves[x]; }
00317   virtual Move_idx_t newKeyFrame() {
00318     Move_idx_t i=moves.new_front();
00319     if(i==invalid_move)
00320       serr->printf("ERROR: MotionSequenceMC %d has run out of memory\n",getID());
00321     return i;
00322   }
00323   virtual void eraseKeyFrame(Move_idx_t x) { moves.erase(x); }
00324   void setRange(unsigned int t,Move_idx_t& prev, Move_idx_t& next) const {
00325     if(next!=invalid_move && moves[next].starttime<=t) {
00326       do {
00327         prev=next;
00328         next=moves[prev].next;
00329       } while(next!=invalid_move && moves[next].starttime<=t);
00330     } else {
00331       while(moves[prev].prev!=invalid_move && t<moves[prev].starttime) {
00332         next=prev;
00333         prev=moves[next].prev;
00334       } 
00335     }
00336   }
00337 };
00338 
00339 /*! @file
00340  * @brief Describes MotionSequence and defines MotionSequenceMC, handy little (or not so little) classes for switching between a sequence of postures
00341  * @author ejt (Creator)
00342  *
00343  * $Author: ejt $
00344  * $Name: tekkotsu-2_2 $
00345  * $Revision: 1.20 $
00346  * $State: Exp $
00347  * $Date: 2004/10/14 19:24:46 $
00348  */
00349 
00350 #endif

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1