Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MotionSequenceEngine.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_MotionSequenceEngine_h_
00003 #define INCLUDED_MotionSequenceEngine_h_
00004 
00005 #include "Shared/LoadSave.h"
00006 #include "IPC/ListMemBuf.h"
00007 #include "PostureEngine.h"
00008 #include "Shared/attributes.h"
00009 
00010 //! A handy class for storing a sequence of keyframed movements
00011 /*! Each outputs is handled independently.  It's easy to add keyframes
00012  *  which modify all of the outputs, but since each output is tracked
00013  *  individually, OutputCmd's with 0 weight can be used to not affect
00014  *  other motions.  For instance, if you want to pan the head left to right while
00015  *  moving the right leg up and down several times, you won't have to
00016  *  specify the intermediary position of the head in its motion at each of the leg
00017  *  motion keyframes.
00018  *
00019  *  Be aware that the 0 time frame will be replaced on a call to
00020  *  play() with the current body posture from ::state.  However, this only applies
00021  *  to outputs which have a non-zero weighted frame defined at some
00022  *  point.  The weights, of the 0 time frame will remain unchanged.  
00023  *  These weights are initially set to 0, so that it's
00024  *  possible to 'fade in' the first frame of the motion sequence from
00025  *  whereever the body happens to be (or already doing)
00026  *
00027  *  To fade out at the end, set a frame with 0 weight for everything.
00028  *  Otherwise it will simply die suddenly.  When a joint reaches its
00029  *  last keyframe, if #hold is set (the default) it will hold its last value
00030  *  until the MotionSequence is reset or stopped.  If #hold is false,
00031  *  then the joint is treated as 0 weight once it reaches its last frame.
00032  *
00033  *  Currently, MotionSequenceEngine is intended mainly for building, 
00034  *  not editing.  It's easy to add keyframes, but hard/impossible to
00035  *  delete them.
00036  *
00037  *  The MotionSequenceEngine base class is an abstract class so that you can
00038  *  create memory efficient motion sequences and simply refer to them
00039  *  by the common base class instead of having to worry about the
00040  *  actual size allocated in the template, MotionSequenceMC.
00041  *
00042  *  @see MotionSequenceEngine::SizeSmall, MotionSequenceEngine::SizeMedium, MotionSequenceEngine::SizeLarge, MotionSequenceEngine::SizeXLarge, 
00043  *  
00044  *  The file format used is as follows: ('<' and '>' are not meant literally)
00045  *  - First line: '<tt>\#MSq</tt>'
00046  *  - Followed by any series of:\n
00047  *    - '<tt>advanceTime </tt><i>time-delta</i>' - moves playhead forward, in milliseconds (synonym for <tt>delay</tt>)
00048  *    - '<tt>delay </tt><i>time-delta</i>' - moves playhead forward, in milliseconds (synonym for <tt>advanceTime</tt>)
00049  *    - '<tt>setTime </tt><i>time</i>' - sets play time to specified value, in ms
00050  *    - '<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[])
00051  *    - '<tt>load </tt><i>filename</i>' - file can be a posture or another motion sequence (if MS, leaves playhead at end of the motion sequence); see setPose() and overlayMotion()
00052  *    - '<tt>loadExplicit </tt><i>filename</i>' - file must be a posture, sets position for all outputs, including zero-weighted ones; see setExplicitPose()
00053  *    - '<tt>#</tt><i>comment</i>' - a comment line
00054  *  - Last line: '<tt>\#END</tt>'
00055  *  
00056  *  Lines beginning with '#' are ignored as comments.  Be aware if you
00057  *  load the file and then save it again, these comments will be lost.
00058  *
00059  *  Example 1: This motion sequence will straighten out the head, panning from right to left.\n
00060 <table align="center"><tr><td align=left><pre>
00061 \#MSq 
00062 
00063 <i>\# Straighten head</i>
00064 advanceTime  50 
00065 NECK:tilt       0.262
00066 NECK:nod       0 
00067 
00068 <i>\# Pan right</i>
00069 advanceTime  850 
00070 NECK:pan~       -0.785 
00071 
00072 <i>\# Pan left</i>
00073 advanceTime  900 
00074 NECK:pan~       0.785 
00075 NECK:tilt       0.262 
00076 NECK:nod       0 
00077 
00078 \#END
00079 </pre></td></tr></table>
00080  *  
00081  *  This graph illustrates the motion of the tilt and pan joints in example 1:
00082  *  <img src="MotionSequenceGraph1.png">
00083  *  Notice how the joint will move from wherever it is initally to the first
00084  *  keyframe <i>for that joint</i>.  Specifying the tilt joint a second time
00085  *  at the end of the motion forces the tilt joint to be held at that position
00086  *  throughout the motion, regardless of the #hold setting at the time
00087  *  the sequence is run.
00088  *  
00089  *  Example 2: This example will straighten the head and the tail, pan the
00090  *  head left to right, and @e then pan the tail left to right.\n
00091 <table align="center"><tr><td align=left><pre>
00092 \#MSq
00093 
00094 <i>\# Bring head and tail to neural positions</i>
00095 advanceTime 50
00096 NECK:pan~ 0
00097 NECK:tilt 0
00098 TAIL:pan~ 0
00099 TAIL:tilt 0
00100 
00101 <i>\# Pan left</i>
00102 advanceTime 1000
00103 NECK:pan~ 1.571
00104 <i>\# Pan right</i>
00105 advanceTime 1000
00106 NECK:pan~ -1.571
00107 
00108 <i>\# Center head</i>
00109 <i>\# Update tail time index</i>
00110 advanceTime 500
00111 NECK:pan~ 0
00112 <i>\# Note this respecification of TAIL:pan~ at 0 -- see graph below</i>
00113 TAIL:pan~ 0
00114 
00115 <i>\# Wag left</i>
00116 advanceTime 500
00117 TAIL:pan~ 1.571
00118 <i>\# Wag right</i>
00119 advanceTime 500
00120 TAIL:pan~ -1.571
00121 
00122 \#END 
00123 </pre></td></tr></table>
00124  *  
00125  *  These graphs illustrate the motion of the pan joint for the head and the tail:
00126  *  <img src="MotionSequenceGraph2-head.png">
00127  *  <img src="MotionSequenceGraph2-tail.png">
00128  *  The head's motion should be straightforward.  The thing to note in the tail's
00129  *  graph is why the second <code>TAIL:pan~ 0</code> is necessary at time
00130  *  2550.  If it were not specified, the tail would slowly move to the left over the
00131  *  course of the head's movement.  We want it to stay still until the head is done,
00132  *  so it must be respecified at the same position to hold it at that value in the
00133  *  intervening time.
00134  *  
00135  *  After loading a motion sequence, the playtime is left at the end.
00136  *  This is to make it easy to append/overlay motion sequences.
00137  *  However, the playhead will be reset to the beginning on the first
00138  *  call to updateOutputs() if isPlaying() returns true.
00139  *
00140  *  You can also create a motion sequence dynamically at run time:
00141  *  \code
00142  *  //This code sample will stand up, point the head forward and up 0.1 radians,
00143  *  //and then autoprune
00144  *
00145  *  //First declare the MotionSequence itself:
00146  *  SharedObject< MotionSequenceMC<MotionSequenceEngine::SizeSmall> > stand;
00147  *
00148  *  //Over the course of the first 700 milliseconds, go to standing posture:
00149  *  standSit->setTime(700);
00150  *  standSit->setPose(PostureEngine("stand.pos"));
00151  *  // could also use standSit->loadFile("stand.pos")
00152  *
00153  *  //Then take another 700 milliseconds to straighten out the head:
00154  *  standSit->advanceTime(700);
00155  *  //We'll set joints individually this time, instead of loading a posture file:
00156  *  standSit->setOutputCmd(HeadOffset+PanOffset,0);
00157  *  standSit->setOutputCmd(HeadOffset+RollOffset,0);
00158  *  standSit->setOutputCmd(HeadOffset+TiltOffset,0.1); //look up .1 radians
00159  *
00160  *  //Add to MotionManager:
00161  *  motman->addPrunableMotion(standSit);
00162  *  //Playback will now begin automatically, and region deallocated when done
00163  *  \endcode
00164  *  
00165  *  By default, #playing is true.  Thus, when you add a MotionSequenceMC
00166  *  to the MotionManager, it will begin executing automatically.  If
00167  *  you do \e not want this behavior, simply call pause() before
00168  *  adding the sequence.
00169  *
00170  *  When the sequence reaches the end, isAlive() will return false.
00171  *  If the motion was added with MotionManager::addPrunableMotion, the
00172  *  motion sequence will then autoprune itself from the MotionManager.
00173  *  However, you can either call MotionManager::addPersistentMotion()
00174  *  to add it, or call setAutoPrune(false), if you want to retain the
00175  *  same instantiation between executions.
00176  *
00177  *  @see PostureEngine for information on the posture files
00178  *  @see <a href="http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/postures.shtml">David Touretzky's "Postures and Motion Sequences" Chapter</a>
00179  *  @see <a href="http://www.cs.cmu.edu/afs/cs/academic/class/15494-s06/www/lectures/postures.pdf">CMU's Cognitive Robotics posture slides</a>
00180  *  @todo We should also have an insertMotion()
00181  */
00182 class MotionSequenceEngine : public LoadSave {
00183 public:
00184   //!constructor, will start playing immediately
00185   MotionSequenceEngine();
00186   //!destructor
00187   virtual ~MotionSequenceEngine() {}
00188 
00189   //! similar to the MotionCommand::updateOutputs, although this isn't a motion command, and doesn't make any calls on MotionManager - merely updates #lasttime, expects subclasses to do the work of sending new commands to the system
00190   virtual int updateOutputs();
00191   
00192   //!@name LoadSave related
00193   virtual unsigned int getBinSize() const; //!< inherited, returns the size used to save the sequence
00194   virtual unsigned int loadBuffer(const char buf[], unsigned int len, const char* filename=NULL); //!< inherited, doesn't clear before loading - call clear yourself if you want to reset, otherwise it will overlay.  Leaves playtime at end of load.
00195   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
00196   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.
00197   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
00198   //! deprecated, use radians instead; calling this will store angles as degrees on future saves
00199   void setSaveDegrees() ATTR_deprecated;
00200   bool isSaveDegrees() const { return loadSaveMode!=1; } //!< returns true if will store angles as degrees on future saves
00201   void setSaveRadians() { loadSaveMode=1; }              //!< will store angles as radians on future saves
00202   bool isSaveRadians() const { return loadSaveMode==1; } //!< returns true if will store angles as degrees on future saves
00203   //@}
00204   
00205   //!@name Sequence Construction
00206   virtual void clear()=0; //!< empties out the sequence (constant time operation - faster than a series of pops)
00207   void setTime(unsigned int x); //!< set the time for both playback and editing (in milliseconds)
00208   unsigned int advanceTime(unsigned int x) {setTime(playtime+x); return playtime; } //!< advance the play/edit index by @a x milliseconds, and then returns the new getTime()
00209   void setOutputCmd(unsigned int i, const OutputCmd& cmd); //!< will insert a keyframe for the given output, or change an existing one
00210   const OutputCmd& getOutputCmd(unsigned int i); //!< gets the value of output @a i at the playhead
00211   void setPose(const PostureEngine& pose); //!< calls setOutputCmd for all non-zero weighted OutputCmds in @a pose (if you wish to set from a file, use loadFile)
00212   void setExplicitPose(const PostureEngine& pose); //!< calls setOutputCmd on each of the OutputCmds in @a pose, even if they are zero-weight (can be used to fade joints in/out with other conflicting motions)
00213   PostureEngine getPose(); //!< returns the set of OutputCmd's at the current playhead as a PostureEngine
00214   void getPose(PostureEngine& pose); //!< stores the set of OutputCmd's at the current playhead into the specified PostureEngine
00215   unsigned int overlayMotion(const std::string& msFile); //!< loads @a msFile from disk and calls overlayMotion(const MotionSequenceEngine&) with it, returns the duration of @a msFile (0 if there was an error)
00216   void overlayMotion(const MotionSequenceEngine& ms); //!< applies each keyframe of @a ms to this, leaves playhead at the end (in other words, advances playhead to end of @a ms)
00217   void compress(); //!< compresses the sequence by eliminating sequences of moves which are identical
00218   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
00219   virtual unsigned int getUsedFrames() const=0; //!< returns the number of used key frames (Move's) which have been stored by the instantiation MotionSequenceEngine subclass
00220   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
00221   //@}
00222 
00223   //!@name Playback Control
00224   bool isPlaying();                                     //! returns true if currently playing
00225   void play();                                          //!< restarts playback from beginning
00226   void pause() { playing=false; }                       //!< pauses playback until another call to play() or resume()
00227   void resume();                                        //!< begins playback from the current playtime
00228   unsigned int getTime() const { return playtime; }     //!< returns the current position of the playback (in milliseconds), see setTime()
00229   unsigned int getEndTime() const { return endtime; }   //!< returns the length of the motion sequence (in milliseconds)
00230   void setSpeed(float x) { playspeed=x; }               //!< sets the playback speed (e.g. 1=regular, 0.5=half speed, -1=@b backwards)
00231   float getSpeed() const { return playspeed; }          //!< returns the playback speed
00232   
00233   virtual void setHold(bool h=true) { hold=h; } //!< Sets #hold - if this is set to false, it will allow a persistent motion to behave the same as a pruned motion, without being pruned
00234   virtual bool getHold() { return hold; }       //!< return #hold
00235   
00236   //@}
00237 
00238 protected:
00239   // TYPES:
00240   typedef unsigned short Move_idx_t; //!< type for indexes to move structures in subclass's storage
00241   static Move_idx_t invalid_move; //!< used to mark the ends of the Move linked lists
00242 
00243   //! This struct holds all the information needed about a frame for a particular output
00244   struct Move {
00245     //!constructor
00246     Move() : cmd(), next(), prev(), starttime(0) {}
00247     OutputCmd cmd;           //!< the actual command to use
00248     Move_idx_t next;        //!< the next frame
00249     Move_idx_t prev;        //!< the previous frame
00250     unsigned int starttime; //!< the time (relative to first frame) this frame should be expressed at
00251   };
00252 
00253   // MEMBERS:
00254   Move_idx_t starts[NumOutputs]; //!< the beginning frame for each output animation
00255   Move_idx_t prevs[NumOutputs];  //!< the previous frame (the starttime for this frame will always be less than or equal to playtime)
00256   Move_idx_t nexts[NumOutputs];  //!< the upcoming frame (the starttime for this frame will always be greater than playtime)
00257   OutputCmd curs[NumOutputs];          //!< merely a cache of current values (if computed, see #curstamps)
00258   unsigned int curstamps[NumOutputs]; //!< timestamp of corresponding value in #curs
00259   unsigned int playtime;              //!< the current time of playback, 0 is start of sequence
00260   unsigned int lasttime;              //!< the time of the last update
00261   unsigned int endtime;               //!< max of moves's Move::starttime's
00262   float playspeed;                    //!< multiplies the difference between current time and starttime, negative will cause play backwards
00263   bool playing;                       //!< true if playing, false if paused
00264   bool hold;                          //!< if set to true, the posture will be kept active; otherwise joints will be marked unused after each posture is achieved (as if the posture was pruned); set through setHold()
00265   
00266   float loadSaveMode;                 //!< 1 to use radians, M_PI/180 for degrees during a save
00267 
00268   virtual Move& getKeyFrame(Move_idx_t x) =0;            //!< returns the Move struct corresponding to @a x in the subclass's actual data structure
00269   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
00270   virtual Move_idx_t newKeyFrame()=0;                    //!< causes subclass to create a new Move structure, returns its index
00271   virtual void eraseKeyFrame(Move_idx_t x)=0;            //!< causes subclass to mark the corresponding Move structure as free
00272 
00273   //!Does the actual calculation of position information.  Perhaps replace with a Bezier or spline or something?
00274   void calcOutput(OutputCmd& ans, unsigned int t, const Move& prev, const Move& next) const {
00275     float prevweight=(float)(next.starttime-t)/(float)(next.starttime-prev.starttime);
00276     ans.set(prev.cmd,next.cmd,prevweight);
00277   }
00278   
00279   //!Sets prev and next to the appropriate values for the given time and output index, return true if there was a change
00280   virtual bool setRange(unsigned int t,Move_idx_t& prev, Move_idx_t& next) const=0;
00281 
00282   //!sets playtime to next time for which any output has a keyframe, -1 if none exists
00283   unsigned int setNextFrameTime(Move_idx_t p[NumOutputs], Move_idx_t n[NumOutputs]) const;
00284   
00285   //!reads a line from a file, parsing it into variables, returns ending position
00286   static unsigned int readWord(const char buf[], const char * const buflen, char word[], const unsigned int wordlen);
00287 
00288   //!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
00289   static unsigned int getOutputIndex(const char name[], unsigned int i);
00290 };
00291 
00292 /*! @file
00293  * @brief Describes MotionSequenceEngine, abstract code for smoothly transitioning between a sequence of postures
00294  * @author ejt (Creator)
00295  */
00296 
00297 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:45 2016 by Doxygen 1.6.3