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  *  @verbatim
00048  *       First line: #MSq
00049  *  Zero or more of: delay <time-delta>              (moves playhead forward, in milliseconds)
00050  *               or: settime <time>                  (sets play time to specified value, in ms)
00051  *               or: <outputname> <value> [<weight>] (sets the specified output to the value - assumes 1 for weight)
00052  *               or: load <filename>                 (file is a posture, sets position)
00053  *               or: overlay <filename>              (file can be a posture or another motion sequence)
00054  *               or: degrees                         (following <value>s will be interpreted as degrees [default])
00055  *               or: radians                         (following <value>s will be interpreted as radians)
00056  *        Last line: #END
00057  *  @endverbatim
00058  *  After loading a motion sequence, the playtime is left at the end.
00059  *  This is to make it easy to append/overlay motion sequences
00060  *  
00061  *  Lines beginning with '#' are ignored.  Output names are defined in RobotInfo.h, RobotInfo::outputNames.
00062  */
00063 class MotionSequence : public MotionCommand, public LoadSave {
00064 public:
00065   //!constructor, will start playing immediately
00066   MotionSequence() : MotionCommand(), playtime(1), lasttime(0), endtime(0), playspeed(1.0), playing(true), loadSaveMode(M_PI/180) {}
00067   //!destructor
00068   virtual ~MotionSequence() {}
00069 
00070   //!To avoid code bloat if there are a large number of different sized MotionSequences, use these sizes where possible.
00071   //!@name Template Sizes
00072   static const unsigned int SizeTiny   = NumOutputs*2;  //!< Tiny, but enough to handle a transition into a full-body pose
00073   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)
00074   static const unsigned int SizeMedium = NumOutputs*6;  //!< Medium (5 full body frames ~ est 4KB)
00075   static const unsigned int SizeLarge  = NumOutputs*11;  //!< Large (10 full body frames ~ est 8KB)
00076   static const unsigned int SizeXLarge = NumOutputs*26;  //!< eXtra Large (25 full body frames ~ est 16KB)
00077   //@}
00078 
00079   //!@name Inherited from MotionCommand
00080   virtual int updateOutputs();
00081   virtual int isDirty() { return isPlaying(); }
00082   virtual int isAlive() { return (playspeed>0) ? (playtime<=endtime) : (playtime>0); }
00083   //@}
00084   
00085   //!@name LoadSave related
00086   virtual unsigned int getBinSize() const; //!< inherited, returns the size used to save the sequence
00087   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.
00088   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
00089   void setSaveDegrees() { loadSaveMode=M_PI/180; }       //!< will store angles as degrees on future saves
00090   bool isSaveDegrees() const { return loadSaveMode!=1; } //!< returns true if will store angles as degrees on future saves
00091   void setSaveRadians() { loadSaveMode=1; }              //!< will store angles as radians on future saves
00092   bool isSaveRadians() const { return loadSaveMode==1; } //!< returns true if will store angles as degrees on future saves
00093   //@}
00094   
00095   //!@name Sequence Construction
00096   virtual void clear()=0; //!< empties out the sequence (constant time operation - faster than a series of pops)
00097   void setPlayTime(unsigned int x); //!< set the time for both playback and editing (in milliseconds)
00098   void setOutputCmd(unsigned int i, const OutputCmd& cmd); //!< will insert a keyframe for the given output, or change an existing one
00099   const OutputCmd& getOutputCmd(unsigned int i); //!< gets the value of output @a i at the playhead
00100   void setPose(const PostureEngine& pose); //!< calls setOutputCmd on each of the OutputCmds in @a pose
00101   void overlayPose(const PostureEngine& pose); //!< calls setOutputCmd on non-zero weighted OutputCmds in @a pose
00102   void compress(); //!< compresses the sequence by eliminating sequences of moves which are identical
00103   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
00104   virtual unsigned int getUsedFrames() const=0; //!< returns the number of used key frames (Move's) which have been stored by the instantiation MotionSequence subclass
00105   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
00106   //@}
00107 
00108   //!@name Playback Control
00109   bool isPlaying() { return playing && isAlive(); }       //!< returns true if currently playing
00110   void play();                                            //!< restarts playback from beginning
00111   void pause() { playing=false; }                         //!< pauses playback until another call to play() or resume()
00112   void resume();                                          //!< begins playback from the current playtime
00113   unsigned int getPlayTime() const { return playtime; }   //!< returns the current position of the playback (in milliseconds), see setPlayTime()
00114   unsigned int getEndTime() const { return endtime; }     //!< returns the length of the motion sequence (in milliseconds)
00115   void setPlaySpeed(float x) { playspeed=x; }            //!< sets the playback speed (e.g. 1=regular, 0.5=half speed, -1=@b backwards)
00116   float getPlaySpeed() const { return playspeed; }       //!< returns the playback speed
00117   //@}
00118 
00119 protected:
00120   // TYPES:
00121   typedef unsigned short Move_idx_t; //!< type for indexes to move structures in #moves
00122   static Move_idx_t invalid_move; //!< used to mark the ends of the Move linked lists
00123 
00124   //! This struct holds all the information needed about a frame for a particular output
00125   struct Move {
00126     //!constructor
00127     Move() : cmd(), next(), prev(), starttime(0) {}
00128     OutputCmd cmd;           //!< the actual command to use
00129     Move_idx_t next;        //!< the next frame
00130     Move_idx_t prev;        //!< the previous frame
00131     unsigned int starttime; //!< the time (relative to first frame) this frame should be expressed at
00132   };
00133 
00134   // MEMBERS:
00135   Move_idx_t starts[NumOutputs]; //!< the beginning frame for each output animation
00136   Move_idx_t prevs[NumOutputs];  //!< the previous frame (the starttime for this frame will always be less than or equal to playtime)
00137   Move_idx_t nexts[NumOutputs];  //!< the upcoming frame (the starttime for this frame will always be greater than playtime)
00138   OutputCmd curs[NumOutputs];          //!< merely a cache of current values (if computed, see #curstamps)
00139   unsigned int curstamps[NumOutputs]; //!< timestamp of corresponding value in #curs
00140   unsigned int playtime;              //!< the current time of playback, 0 is start of sequence
00141   unsigned int lasttime;              //!< the time of the last update
00142   unsigned int endtime;               //!< max of #moves's Move::starttime's
00143   float playspeed;                    //!< multiplies the difference between current time and starttime, negative will cause play backwards
00144   bool playing;                       //!< true if playing, false if paused
00145   
00146   float loadSaveMode;                 //!< 1 to use radians, M_PI/180 for degrees during a save
00147 
00148   virtual Move& getKeyFrame(Move_idx_t x) =0;            //!< returns the Move struct corresponding to @a x in the subclass's actual data structure
00149   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
00150   virtual Move_idx_t newKeyFrame()=0;                    //!< causes subclass to create a new Move structure, returns its index
00151   virtual void eraseKeyFrame(Move_idx_t x)=0;            //!< causes subclass to mark the corresponding Move structure as free
00152 
00153   //!Does the actual calculation of position information.  Perhaps replace with a Bezier or spline or something?
00154   void calcOutput(OutputCmd& ans, unsigned int t, const Move& prev, const Move& next) const {
00155     float prevweight=(float)(next.starttime-t)/(float)(next.starttime-prev.starttime);
00156     ans.set(prev.cmd,next.cmd,prevweight);
00157   }
00158   
00159   //!Sets prev and next to the appropriate values for the given time and output index
00160   virtual void setRange(unsigned int t,Move_idx_t& prev, Move_idx_t& next) const=0;
00161 
00162   //!used by LoadBuffer()/SaveBuffer(), checks to see if the amount read/written (@a res) is nonzero, increments @a buf, decrements @a len, or displays @a msg if @a is zero
00163   static bool ChkAdvance(int res, const char** buf, unsigned int* len, const char* msg);
00164 
00165   //!used by LoadBuffer()/SaveBuffer(), checks to see if the amount read/written (@a res) is nonzero, increments @a buf, decrements @a len, or displays @a msg with @a arg1 if @a is zero
00166   static bool ChkAdvance(int res, const char** buf, unsigned int* len, const char* msg, int arg1);
00167 
00168   //!sets playtime to next time for which any output has a keyframe, -1 if none exists
00169   unsigned int setNextFrameTime(Move_idx_t p[NumOutputs], Move_idx_t n[NumOutputs]) const;
00170   
00171   //!reads a line from a file, parsing it into variables, returns ending position
00172   static unsigned int readWord(const char buf[], const char * const buflen, char word[], const unsigned int wordlen);
00173 
00174   //!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
00175   static unsigned int getOutputIndex(const char name[], unsigned int i);
00176 };
00177 
00178 //! Instantiates MotionSequences - when you want to make a new MotionSequence, make one of these
00179 /*! Allows a (compile-time) variable amount of data storage through its template parameter.
00180  *  See MotionSequence for documentation on its members
00181  *  @see MotionSequence
00182  *  @see MotionSequence::SizeSmall, MotionSequence::SizeMedium, MotionSequence::SizeLarge, MotionSequence::SizeXLarge, 
00183  *  */
00184 template<unsigned int MAXMOVE>
00185 class MotionSequenceMC : public MotionSequence {
00186 public:
00187   //!constructor
00188   MotionSequenceMC() : MotionSequence(), moves() {clear();}
00189   //!constructor, loads from a file and then resets the playtime to beginning and begins to play
00190   explicit MotionSequenceMC(const char* filename) : MotionSequence(), moves() {clear();LoadFile(filename);setPlayTime(1);}
00191   //!destructor
00192   virtual ~MotionSequenceMC() {}
00193 
00194   
00195   /*struct logent {
00196     logent(unsigned int t, unsigned int p, unsigned int i, OutputCmd f[NumFrames])
00197       : time(t), play(p), index(i)
00198     {
00199       for(unsigned int j=0;j<NumFrames;j++)
00200         frames[j]=f[j];
00201     }
00202     unsigned int time,play,index;
00203     OutputCmd frames[NumFrames];
00204   };
00205 
00206   std::vector<logent> log;
00207 
00208   virtual int isAlive() {
00209     if(MotionSequenceMC::isAlive())
00210       return true;
00211     cout << "LOG:" << endl;
00212     for(unsigned int i=0; i<log.size(); i++) {
00213       cout << '#' << '\t' << log[i].time << '\n';
00214       for(unsigned int j=0; j<NumFrames; j++)
00215         cout << log[i].play+j*FrameTime << ' ' << log[i].frames[j].value << ' ' << log[i].frames[j].weight << ' ' << log[i].index << '\n';
00216     }
00217     cout << endl;
00218     log.clear();
00219     return false;
00220     }*/
00221 
00222   // I put this here because i want direct access to moves so it'll be faster
00223   virtual int updateOutputs() {
00224     MotionSequence::updateOutputs();
00225     if(!isPlaying()) {
00226       for(unsigned int i=0; i<NumOutputs; i++) //just copies getOutputCmd(i) across frames
00227         motman->setOutput(this,i,getOutputCmd(i));
00228     } else {
00229       for(unsigned int i=0; i<NumOutputs; i++) { //fill out the buffer of commands for smoother movement
00230         Move_idx_t prev=prevs[i],next=nexts[i];
00231         OutputCmd frames[NumFrames];
00232         frames[0]=getOutputCmd(i);
00233         for(unsigned int t=playtime+FrameTime,j=1;j<NumFrames;j++,t+=FrameTime) {
00234           setRange(t,prev,next);
00235           if(next!=invalid_move)
00236             calcOutput(frames[j],t,moves[prev],moves[next]);
00237           else
00238             frames[j].unset();
00239         }
00240         motman->setOutput(this,i,frames);
00241       }
00242     }
00243     return NumOutputs;
00244     //    if(i<NumLegJointss)
00245     //      log.push_back(logent(get_time(),playtime,i,frames));
00246   }
00247   
00248   virtual void clear() {
00249     moves.clear();
00250     for(unsigned int i=0; i<NumOutputs; i++) {
00251       prevs[i]=starts[i]=moves.new_back();
00252       moves.back().cmd.unset();
00253       moves.back().next=invalid_move;
00254       moves.back().prev=invalid_move;
00255       nexts[i]=invalid_move;
00256     }
00257     setPlayTime(1);
00258   }
00259   virtual unsigned int getMaxFrames() const { return moves.getMaxCapacity(); }
00260   virtual unsigned int getUsedFrames() const { return moves.size(); }
00261 
00262 protected:
00263   // TYPES:
00264   typedef ListMemBuf<Move,MAXMOVE,Move_idx_t> list_t; //!< shorthand for the ListMemBuf that stores all of the movement frames
00265 
00266   // MEMBERS:
00267   list_t moves;                       //!< stores all of the movement keyframes
00268 
00269   virtual Move& getKeyFrame(Move_idx_t x) { return moves[x]; }
00270   virtual const Move& getKeyFrame(Move_idx_t x) const { return moves[x]; }
00271   virtual Move_idx_t newKeyFrame() { return moves.new_front(); }
00272   virtual void eraseKeyFrame(Move_idx_t x) { moves.erase(x); }
00273   void setRange(unsigned int t,Move_idx_t& prev, Move_idx_t& next) const {
00274     if(next!=invalid_move && moves[next].starttime<=t) {
00275       do {
00276         prev=next;
00277         next=moves[prev].next;
00278       } while(next!=invalid_move && moves[next].starttime<=t);
00279     } else if(t<moves[prev].starttime) {
00280       do {
00281         next=prev;
00282         prev=moves[next].prev;
00283       } while(t<moves[prev].starttime);
00284     }
00285   }
00286 };
00287 
00288 /*! @file
00289  * @brief Describes MotionSequence and defines MotionSequenceMC, handy little (or not so little) classes for switching between a sequence of postures
00290  * @author ejt (Creator)
00291  *
00292  * $Author: ejt $
00293  * $Name: tekkotsu-2_1 $
00294  * $Revision: 1.12 $
00295  * $State: Exp $
00296  * $Date: 2004/02/19 23:08:09 $
00297  */
00298 
00299 #endif

Tekkotsu v2.1
Generated Tue Mar 16 23:19:14 2004 by Doxygen 1.3.5