Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
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 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-1_4_1 $ 00293 * $Revision: 1.9 $ 00294 * $State: Exp $ 00295 * $Date: 2003/06/10 04:41:41 $ 00296 */ 00297 00298 #endif
Tekkotsu v1.4 |
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2 |