Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
MotionSequenceMC.hGo 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 |