Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
PostureMC.hGo 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 |