Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PostureMC.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_PostureMC_h
00003 #define INCLUDED_PostureMC_h
00004 
00005 #include "MotionCommand.h"
00006 #include "PostureEngine.h"
00007 
00008 class WorldState;
00009 
00010 //! a MotionCommand shell for PostureEngine
00011 /*! Will autoprune by default once it reaches the target pose.
00012  * 
00013  *  If you want to keep it alive so your behavior can repeatedly use
00014  *  the posture to move around, either call setAutoPrune(@c false), or
00015  *  (preferably) use MotionManager::addPersistentMotion() when adding
00016  *  the motion.
00017  *
00018  *  This class will throw a status event when the sensors confirm the
00019  *  robot has reached a specified target posture, within a configurable
00020  *  range of #tolerance.  The class will keep trying to reach a target
00021  *  position for #timeout milliseconds before giving up.
00022  *
00023  *  By default, this class will adhere to the values of
00024  *  ::MaxOutputSpeed, except for head joints which use
00025  *  Config::motion_config values.
00026  */
00027 class PostureMC : public MotionCommand, public PostureEngine {
00028 public:
00029   //!timeout will be set to a default value of 2 seconds, tolerance is .035 radians (~2 degrees), hold is true
00030   //!@name Constructors
00031 
00032   //!constructor
00033   PostureMC()
00034     : MotionCommand(),PostureEngine(),dirty(true),hold(true),tolerance(.035),
00035       targetReached(false),targetTimestamp(0),timeout(2000)
00036   { init(); }
00037   
00038   //!constructor, loads from @a filename
00039   PostureMC(const std::string& filename)
00040     : MotionCommand(),PostureEngine(filename),dirty(true),hold(true),tolerance(.035),
00041       targetReached(false),targetTimestamp(0),timeout(2000)
00042   { init(); }
00043   
00044   //!constructor, initializes joint positions to the current state of the outputs as defined by @a state
00045   PostureMC(const WorldState* st)
00046     : MotionCommand(),PostureEngine(st),dirty(true),hold(true),tolerance(.035),
00047       targetReached(false),targetTimestamp(0),timeout(2000)
00048   { init(); }
00049   
00050   //!destructor
00051   virtual ~PostureMC() {}
00052   //@}
00053   
00054   //!These functions allow you to mediate problems with a physical robot, such as tolerance of joints not going quite where the should, or safe speed of movement
00055   //!@name Physical Implementation Issues
00056 
00057   //!Call this if you call PostureEngine::setOutputCmd(), or set values through getOutputCmd()'s reference, since neither will know to set the PostureMC dirty flag
00058   /*! This is the downside of making setOutputCmd() not virtual - if
00059    *  you pass this around as just a PostureEngine, calls made to that
00060    *  won't be able to update the dirty flag automatically.  However,
00061    *  if the object is typed as a PostureMC at the time the call is
00062    *  made, then you don't have to worry.\n These are also not virtual
00063    *  - otherwise you'd have to still check the motion out and it
00064    *  would make this all pointless!
00065    *
00066    *  MotionManager::getOutputCmd() is called instead of
00067    *  WorldState::outputs[] because if this is being called rapidly
00068    *  (i.e. after every sensor reading) using the sensor values will
00069    *  cause problems with very slow acceleration due to sensor lag
00070    *  continually resetting the current position.  Using the last
00071    *  value sent by the MotionManager fixes this.
00072    *
00073    * @return @c *this */
00074   PostureMC& setDirty(bool d=true);
00075 
00076   //!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
00077   virtual PostureMC& setHold(bool h=true) { hold=h; return *this; }
00078   virtual bool getHold() { return hold; } //!< return #hold
00079 
00080   virtual PostureMC& setTolerance(float t) { tolerance=t; return *this; } //!< sets #tolerance, returns @c *this
00081   virtual float getTolerance() { return tolerance; } //!< returns #tolerance
00082   virtual PostureMC& setTimeout(unsigned int delay) { timeout=delay; return *this; } //!< sets #timeout, returns @c *this
00083   virtual unsigned int getTimeout() { return timeout; } //!< returns #timeout
00084 
00085   //! Sets #maxSpeeds to 0 (no maximum)
00086   void noMaxSpeed() { for(unsigned int i=0; i<NumOutputs; i++) maxSpeeds[i]=0; }
00087   
00088   //! Restores #maxSpeeds to default ::MaxOutputSpeed values from the robot info file (e.g. ERS7Info.h)
00089   /*! Neck joints are instead set from the applicable
00090    *  Config::motion_config values.
00091    *  @param x ratio of the max speed to use; so 0.5 would limit motion to half the recommended upper limit */
00092   void defaultMaxSpeed(float x=1);
00093   
00094   //! Sets #maxSpeeds[i] entry, in rad/sec
00095   /*! @param i joint offset, see your model's info file (e.g. ERS7Info.h)
00096    *  @param x maximum radians per second to move */
00097   void setMaxSpeed(unsigned int i, float x) { maxSpeeds[i]=x*FrameTime/1000; }
00098   
00099   //! Returns #maxSpeeds[i] entry, in rad/sec
00100   /*! @param i joint offset, see your model's info file (e.g. ERS7Info.h)
00101    *  @return the maximum speed of joint @a i in radians per second */
00102   float getMaxSpeed(unsigned int i) { return maxSpeeds[i]*1000/FrameTime; }
00103   
00104   //@}
00105 
00106 
00107 
00108   //!@name MotionCommand Stuff
00109   virtual int updateOutputs();
00110   virtual int isDirty() { return dirty || !targetReached; }
00111 
00112   //! returns non-zero (true) if PostureEngine::maxdiff() between this and the current position is over #tolerance
00113   /*! This is handy so you can set to have the robot go to a position and then automatically remove
00114    *  the MotionCommand when it gets there - but beware fighting Postures which average out and neither
00115    *  succeeds */
00116   virtual int isAlive();
00117   virtual void DoStart() { MotionCommand::DoStart(); dirty=true; } //!< marks this as dirty each time it is added
00118   //@}
00119 
00120   //!These functions merely set the dirty flag and then call the PostureEngine version of the function
00121   //!@name PostureEngine Stuff
00122   inline virtual void takeSnapshot() { setDirty(); PostureEngine::takeSnapshot(); }
00123   inline virtual void takeSnapshot(const WorldState* st) { setDirty(); PostureEngine::takeSnapshot(st); }
00124   inline virtual void setWeights(float w) { setWeights(w,0,NumOutputs); }
00125   inline virtual void setWeights(float w, unsigned int lowjoint, unsigned int highjoint) { setDirty(); PostureEngine::setWeights(w,lowjoint,highjoint); }
00126   inline virtual void clear() { setDirty(); PostureEngine::clear(); }
00127   inline virtual PostureEngine& setOverlay(const PostureEngine& pe) { setDirty(); PostureEngine::setOverlay(pe); return *this; }
00128   inline virtual PostureEngine& setUnderlay(const PostureEngine& pe) { setDirty(); PostureEngine::setUnderlay(pe); return *this; }
00129   inline virtual PostureEngine& setAverage(const PostureEngine& pe,float w=0.5) { setDirty(); PostureEngine::setAverage(pe,w); return *this; }
00130   inline virtual PostureEngine& setCombine(const PostureEngine& pe) { setDirty(); PostureEngine::setCombine(pe); return *this; }
00131   inline PostureEngine& setOutputCmd(unsigned int i, const OutputCmd& c) { setDirty(); PostureEngine::setOutputCmd(i,c); return *this; }
00132   inline virtual unsigned int LoadBuffer(const char buf[], unsigned int len) { setDirty(); return PostureEngine::LoadBuffer(buf,len); }
00133   inline virtual bool solveLinkPosition(const NEWMAT::ColumnVector& Ptgt, unsigned int link, const NEWMAT::ColumnVector& Peff) { setDirty(); return PostureEngine::solveLinkPosition(Ptgt,link,Peff); }
00134   inline virtual bool solveLinkPosition(float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z) { setDirty(); return PostureEngine::solveLinkPosition(Ptgt_x,Ptgt_y,Ptgt_z,link,Peff_x,Peff_y,Peff_z); }
00135   inline virtual bool solveLinkVector(const NEWMAT::ColumnVector& Ptgt, unsigned int link, const NEWMAT::ColumnVector& Peff) { setDirty(); return PostureEngine::solveLinkVector(Ptgt,link,Peff); }
00136   inline virtual bool solveLinkVector(float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z) { setDirty(); return PostureEngine::solveLinkVector(Ptgt_x,Ptgt_y,Ptgt_z,link,Peff_x,Peff_y,Peff_z); }
00137   //@}
00138 
00139 protected:
00140   void init(); //!< initialize #curPositions and #maxSpeeds
00141 
00142   bool  dirty;                    //!< true if changes have been made since last updateOutputs()
00143   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()
00144   float tolerance;                //!< when autopruning, if the maxdiff() of this posture and the robot's current position is below this value, isAlive() will be false, defaults to 0.01 (5.7 degree error)
00145   bool  targetReached;            //!< false if any joint is still moving towards its target
00146   unsigned int targetTimestamp;   //!< time at which the targetReached flag was set
00147   unsigned int timeout;           //!< number of milliseconds to wait before giving up on a target that should have already been reached, a value of -1U will try forever
00148   float curPositions[NumOutputs]; //!< stores the last commanded value for each joint
00149   float maxSpeeds[NumOutputs];    //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame
00150 };
00151 
00152 /*! @file
00153  * @brief Describes PostureMC, a MotionCommand shell for PostureEngine
00154  * @author ejt (Creator)
00155  *
00156  * $Author: ejt $
00157  * $Name: tekkotsu-2_4_1 $
00158  * $Revision: 1.13 $
00159  * $State: Exp $
00160  * $Date: 2005/08/07 04:11:03 $
00161  */
00162 
00163 #endif

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4