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

Tekkotsu v2.0
Generated Wed Jan 21 03:20:29 2004 by Doxygen 1.3.4