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

Tekkotsu v4.0
Generated Thu Nov 22 00:54:55 2007 by Doxygen 1.5.4