Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

DynamicMotionSequence.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_DynamicMotionSequence_h_
00003 #define INCLUDED_DynamicMotionSequence_h_
00004 
00005 #include "MotionSequenceEngine.h"
00006 #include "MotionCommand.h"
00007 #include "MotionManager.h"
00008 #include "Events/EventBase.h"
00009 #include <vector>
00010 
00011 //! Uses STL's vector for dynamic memory allocation - don't use this as a motion command, pointers in shared memory regions can be invalid in other processes
00012 /*! See MotionSequenceEngine for documentation on its members */
00013 class DynamicMotionSequence : public MotionCommand, public MotionSequenceEngine {
00014 public:
00015 
00016   //!constructor
00017   DynamicMotionSequence() : MotionCommand(), MotionSequenceEngine(), moves(), erased(), dirty(false) { clear(); }
00018 
00019   //!constructor, loads from a file and then resets the playtime to beginning and begins to play
00020   explicit DynamicMotionSequence(const char* filename) : MotionCommand(), MotionSequenceEngine(), moves(), erased(), dirty(false) {
00021     clear();
00022     loadFile(filename);
00023     setTime(1);
00024   }
00025 
00026   //!destructor
00027   virtual ~DynamicMotionSequence() {}
00028 
00029   // I put this here because i want direct access to moves so it'll be faster
00030 /*  void planJointCmds(unsigned int i, JointCmd frames[NumFrames]) {
00031     Move_idx_t prev=prevs[i],next=nexts[i];
00032     frames[0]=getJointCmd(i);
00033     for(unsigned int t=playtime+FrameTime,j=1;j<NumFrames;j++,t+=FrameTime) {
00034       setRange(t,prev,next);
00035       if(next!=invalid_move)
00036         calcJoint(frames[j],t,moves[prev],moves[next]);
00037       else
00038         frames[j]=unusedJoint;
00039     }
00040   }*/
00041   
00042 
00043   virtual int isDirty() { return isPlaying(); }
00044   virtual int isAlive() { return (playspeed>0) ? (playtime<=endtime) : (playtime>0); }
00045   
00046   void setDirty() { dirty = true; };
00047 
00048   virtual int updateOutputs() {
00049     MotionSequenceEngine::updateOutputs();
00050     if(!isPlaying()) {
00051       if(dirty)
00052         postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00053       dirty=false;
00054       for(unsigned int i=0; i<NumOutputs; i++) //just copies getOutputCmd(i) across frames
00055         motman->setOutput(this,i,getOutputCmd(i));
00056     } else {
00057       dirty=true;
00058       for(unsigned int i=0; i<NumOutputs; i++) { //fill out the buffer of commands for smoother movement
00059         Move_idx_t prev=prevs[i],next=nexts[i];
00060         OutputCmd frames[NumFrames];
00061         frames[0]=getOutputCmd(i);
00062         for(unsigned int t=playtime+FrameTime,j=1;j<NumFrames;j++,t+=FrameTime) {
00063           setRange(t,prev,next);
00064           if(next!=invalid_move)
00065             calcOutput(frames[j],t,moves[prev],moves[next]);
00066           else
00067             frames[j].unset();
00068         }
00069         motman->setOutput(this,i,frames);
00070       }
00071     }
00072     return NumOutputs;
00073     //    if(i<NumLegJointss)
00074     //      log.push_back(logent(get_time(),playtime,i,frames));
00075   }
00076   
00077   virtual void clear() {
00078     moves.clear();
00079     erased.clear();
00080     for(unsigned int i=0; i<NumOutputs; i++) {
00081       moves.push_back(Move());
00082       moves.back().cmd.unset();
00083       moves.back().next=invalid_move;
00084       moves.back().prev=invalid_move;
00085       prevs[i]=starts[i]=moves.size()-1;
00086       nexts[i]=invalid_move;
00087     }
00088     setTime(1);
00089   }
00090   virtual unsigned int getMaxFrames() const { return -1U; }
00091   virtual unsigned int getUsedFrames() const { return moves.size()-erased.size(); }
00092 
00093 protected:
00094   // TYPES:
00095   typedef std::vector<Move> list_t; //!< shorthand for the ListMemBuf that stores all of the movement frames
00096 
00097   // MEMBERS:
00098   list_t moves;                     //!< stores all of the movement keyframes
00099   std::vector<Move_idx_t> erased;   //!< recycles erased keyframes, can't just shift elements in #moves, it would throw off index numbers in Move structures
00100   bool dirty; //!< true if last updateOutputs was dirty, so we know when to post status event
00101 
00102   virtual Move& getKeyFrame(Move_idx_t x) { return moves[x]; } //!< returns #moves[@a x]
00103   virtual const Move& getKeyFrame(Move_idx_t x) const { return moves[x]; } //!< returns #moves[@a x]
00104   virtual Move_idx_t newKeyFrame() {
00105     if(erased.empty()) {
00106       moves.push_back(Move());
00107       return moves.size()-1;
00108     } else { //recycle from used list
00109       Move_idx_t x=erased.back();
00110       erased.pop_back();
00111       return x;
00112     }
00113   }
00114   //! marks keyframe @a x unused
00115   virtual void eraseKeyFrame(Move_idx_t x) { erased.push_back(x); }
00116   //! advances (or rewinds) @a prev and @a next so that @a t falls between them
00117   bool setRange(unsigned int t,Move_idx_t& prev, Move_idx_t& next) const {
00118     bool moved=false;
00119     if(next!=invalid_move && moves[next].starttime<=t) {
00120       moved=true;
00121       do {
00122         prev=next;
00123         next=moves[prev].next;
00124       } while(next!=invalid_move && moves[next].starttime<=t);
00125     } else {
00126       while(moves[prev].prev!=invalid_move && t<moves[prev].starttime) {
00127         next=prev;
00128         prev=moves[next].prev;
00129         moved=true;
00130       }
00131     }
00132     return moved;
00133   }
00134 };
00135 
00136 /*! @file
00137  * @brief Uses STL's vector for dynamic memory allocation - don't use this as a motion command, pointers in shared memory regions can be invalid in other processes
00138  * @author ejt (Creator)
00139  */
00140 
00141 #endif

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