Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
PostureEngine.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_PostureEngine_h 00003 #define INCLUDED_PostureEngine_h 00004 00005 #include "Motion/OutputCmd.h" 00006 #include "Motion/Kinematics.h" 00007 #include "Shared/RobotInfo.h" 00008 #include "Shared/LoadSave.h" 00009 00010 class WorldState; 00011 00012 //! A class for storing a set of positions and weights for all the outputs 00013 /*! File Format: ([..] indicates an optional parameter)\n 00014 * - First line: '<tt>\#POS</tt>' 00015 * - Followed by a series of: 00016 * - '<tt>specialize </tt><i>[regex]</i>' - only robots whose model name matches the regular 00017 * expression will attempt to parse lines between this and the next 'specialize' command. An empty 00018 * (absent) regex matches all robots (equivalent to '<tt>specialize .*</tt>') 00019 * - '<tt>condensed </tt><i>model</i>' - specifies model name for which condensed specifications will be 00020 * interpreted 00021 * - '<tt>verbose</tt>' - switches the load/save format back to "verbose" style, where each output/sensor 00022 * value is listed individually 00023 * - '<tt><</tt><i>section</i><tt>></tt> ... <tt></</tt><i>section</i><tt>></tt>' - specifies a 00024 * section for following "verbose" style lines (valid <i>section</i> values listed below) 00025 * - '<i>section</i> <i>value1</i> <i>value2</i> <i>value3</i> ...' - specify all values for a section in 00026 * one line, "condensed" style (valid <i>section</i> values listed below). Must have a value for every 00027 * item in the section (no extra or missing values) 00028 * - '<i>output-name</i> <i>value</i> <i>[weight]</i>' - specifies an output value, with optional weighting 00029 * value (if no weight specified, '1' is assumed) 00030 * - Last line: '<tt>\#END</tt>' 00031 * 00032 * Note that '=' can be used to separate tokens as well as any whitespace character. 00033 * All angle values should be specified in radians. 00034 * 00035 * Valid section names are: 00036 * - <tt>meta-info</tt> - only recognizes two fields: <tt>timestamp</tt> and <tt>framenumber</tt> 00037 * - <tt>outputs</tt> - supports all output names (e.g. ERS7Info::outputNames) 00038 * - <tt>buttons</tt> - supports all button names (e.g. ERS7Info::buttonNames) 00039 * - <tt>sensors</tt> - supports all sensor names (e.g. ERS7Info::sensorNames) 00040 * - <tt>pidduties</tt> - supports output names corresponding to PID joints (this is the "duty cycle" for the joint) 00041 * 00042 * Additionally 'weights' can be used as a condensed section name (but not a 00043 * verbose tag-style section, since weights are specified on each line). The 00044 * 'weights' specification must follow the 'outputs' specification to be effective. 00045 * 00046 * Data following '\#' is ignored as comments. Be aware if you 00047 * load the file and then save it again, these comments will be lost. 00048 * 00049 * Example 1: Specifies only neck joints, looks forward and up a little 00050 <table><tr><td align=left><pre> 00051 \#POS 00052 NECK:tilt 0 00053 NECK:pan 0 00054 NECK:nod 0.2 00055 \#END 00056 </pre></td></tr></table> 00057 * All other, unspecified, joints will be set to weight=0. 00058 * 00059 * Example 2: Logged sensor data from an ERS-7 robot 00060 <table><tr><td align=left><tt> 00061 \#POS<br> 00062 condensed ERS-7<br> 00063 meta-info = 1439041 178803<br> 00064 outputs = -0.131722 0.148077 1.74592 -0.30276 0.341717 2.13361 -1.03935 0.091124 2.18958 -0.804097 0.034171 1.67458 -0.016362 -0.089143 0.125563 0.407243 -0.054399 -0.064704 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0<br> 00065 buttons = 0 0 0 0 0 0 0 0 0 1<br> 00066 sensors = 500 860.139 425 -2.06456 -0.516139 -8.94642 0.99 30.63 2200 8.265 0<br> 00067 pidduties = -0.164062 0.0878906 0.0957031 0.246094 -0.0195312 -0.0546875 -0.164062 -0.0195312 0.164062 0.0273438 0 -0.0683594 -0.00585938 -0.00390625 0.0820312 -0.0390625 0.015625 0.00390625<br> 00068 \#END<br> 00069 </tt></td></tr></table> 00070 * Condensed posture files can still be loaded on other models. For each entry, RobotInfo::Capabilities will be 00071 * used to map from the model specified by the <tt>condensed</tt> command to the current host. 00072 * 00073 * @see PostureMC 00074 * @see <a href="http://www.cs.cmu.edu/~tekkotsu/Kinematics.html">Tekkotsu's Kinematics page</a> 00075 * @see <a href="http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/postures.shtml">David Touretzky's "Postures and Motion Sequences" Chapter</a> 00076 * @see <a href="http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/forwardkin.shtml">David Touretzky's "Forward Kinematics" Chapter</a> 00077 * @see <a href="http://www.cs.cmu.edu/afs/cs/academic/class/15494-s06/www/lectures/postures.pdf">CMU's Cognitive Robotics posture slides</a> 00078 * @see <a href="http://www.cs.cmu.edu/afs/cs/academic/class/15494-s06/www/lectures/kinematics.pdf">CMU's Cognitive Robotics kinematics slides</a> 00079 */ 00080 class PostureEngine : public LoadSave, public Kinematics { 00081 public: 00082 00083 //!@name Constructors 00084 00085 //!constructor 00086 PostureEngine() : LoadSave(), Kinematics(*kine), saveFormatCondensed(false), saveSensors(NULL), loadSensors(NULL) {} 00087 //!constructor, loads a position from a file 00088 /*! @todo might want to make a library stored in memory of common positions so they don't have to be loaded repeatedly from memstick */ 00089 PostureEngine(const std::string& filename) : LoadSave(), Kinematics(*kine), saveFormatCondensed(false), saveSensors(NULL), loadSensors(NULL) { loadFile(filename.c_str()); } 00090 //!constructor, initializes joint positions to the current state of the outputs as defined by @a state 00091 PostureEngine(const WorldState* st) : LoadSave(), Kinematics(*kine), saveFormatCondensed(false), saveSensors(NULL), loadSensors(NULL) { if(st!=NULL) takeSnapshot(*st); } 00092 00093 //! copy constructor 00094 PostureEngine(const PostureEngine& pe) 00095 : LoadSave(pe), Kinematics(pe), saveFormatCondensed(pe.saveFormatCondensed), saveSensors(pe.saveSensors), loadSensors(NULL) 00096 { 00097 for(unsigned int i=0; i<NumOutputs; i++) 00098 cmds[i]=pe.cmds[i]; 00099 } 00100 00101 //! assignment operator 00102 PostureEngine& operator=(const PostureEngine& pe) { 00103 LoadSave::operator=(pe); 00104 Kinematics::operator=(pe); 00105 saveFormatCondensed=pe.saveFormatCondensed; 00106 saveSensors=pe.saveSensors; 00107 for(unsigned int i=0; i<NumOutputs; i++) 00108 cmds[i]=pe.cmds[i]; 00109 return *this; 00110 } 00111 00112 //! destructor 00113 virtual ~PostureEngine(); 00114 //@} 00115 00116 00117 00118 //! You should be able to call the non-virtual functions without checking out, just a MotionManager::peekMotion(). Theoretically. 00119 //!@name Output Value Access/Control 00120 virtual void takeSnapshot(); //!< sets the values of #cmds to the current state of the outputs (doesn't change the weights) 00121 virtual void takeSnapshot(const WorldState& st); //!< sets the values of #cmds to the current state of the outputs as defined by @a state (doesn't change the weights) 00122 virtual void setWeights(float w) { setWeights(w,0,NumOutputs); } //!< set the weights of all #cmds 00123 virtual void setWeights(float w, unsigned int lowjoint, unsigned int highjoint); //!< the the weights of a range of #cmds 00124 virtual void clear(); //!< sets all joints to unused 00125 inline PostureEngine& setOutputCmd(unsigned int i, const OutputCmd& c) { cmds[i]=c; return *this; } //!<sets output @a i to OutputCmd @a c, returns @c *this so you can chain them; also remember that OutputCmd support implicit conversion from floats (so you can just pass a float) 00126 inline OutputCmd& operator()(unsigned int i) { return cmds[i]; } //!< returns output @a i, returns a reference so you can also set through an assignment to this call, e.g. pose(MouthOffset)=.1; (remember that OutputCmd support implicit conversion from floats) 00127 inline const OutputCmd& operator()(unsigned int i) const { return cmds[i]; } //!< returns output @a i 00128 inline OutputCmd& getOutputCmd(unsigned int i) { return cmds[i]; } //!< returns output @a i, returns a reference so you can also set through an assignment 00129 inline const OutputCmd& getOutputCmd(unsigned int i) const { return cmds[i]; } //!< returns output @a i 00130 //@} 00131 00132 00133 00134 //!Uses LoadSave interface so you can load/save to files, uses a human-readable storage format 00135 //!@name LoadSave 00136 virtual void setSaveFormat(bool condensed, WorldState* ws); //!< sets #saveFormatCondensed and #loadSaveSensors (pass ::state for @a ws if you want to use current sensor values) 00137 virtual unsigned int getBinSize() const; 00138 virtual unsigned int loadBuffer(const char buf[], unsigned int len, const char* filename=NULL); 00139 virtual unsigned int saveBuffer(char buf[], unsigned int len) const; 00140 virtual unsigned int loadFile(const char filename[]); 00141 virtual unsigned int saveFile(const char filename[]) const; 00142 00143 //! used for storing sensor data loaded from file 00144 struct SensorInfo { 00145 SensorInfo() : buttons(), sensors(), pidduties(), timestamp(-1U), frameNumber(-1U) {} 00146 std::map<unsigned int,float> buttons; //!< button values 00147 std::map<unsigned int,float> sensors; //!< sensor values 00148 std::map<unsigned int,float> pidduties; //!< pid duty cycle values, indices are relative to PIDJointOffset 00149 unsigned int timestamp; //!< timestamp associated with sensor values, or -1U if none found 00150 unsigned int frameNumber; //!< sensor frame number associated with sensor values, or -1U if none found 00151 }; 00152 virtual void setLoadedSensors(SensorInfo* si) { loadSensors=si; } //!< if @a ws is non-NULL, any sensor values in loaded postures will be stored there (otherwise they are ignored) 00153 virtual SensorInfo* getLoadedSensors() const { return loadSensors; } //!< returns value previously stored by setLoadSensors() 00154 //@} 00155 00156 00157 00158 //!@name Kinematics 00159 00160 //! Performs inverse kinematics to solve for positioning @a Peff on link @a j as close as possible to @a Ptgt (base coordinates in homogenous form); if solution found, stores result in this posture and returns true 00161 /*! @param Ptgt the target point, in base coordinates 00162 * @param link the output offset of the joint to move 00163 * @param Peff the point (relative to @a link) which you desire to have moved to @a Ptgt (it's a point on the effector, e.g. (0,0,0) if you want to move the effector's origin to the target) 00164 * 00165 * The difference between solveLinkPosition() and solveLinkVector() is typically small, 00166 * but critical when you're trying to look at something -- the solution obtained by 00167 * simplying trying to solve for the position may not align the vector with the target -- 00168 * solveLinkVector() tries to ensure the vector is aligned with the target, even if that 00169 * isn't the closest solution position-wise. 00170 */ 00171 virtual bool solveLinkPosition(const fmat::SubVector<3,const float>& Ptgt, unsigned int link, const fmat::SubVector<3,const float>& Peff); 00172 00173 //! Performs inverse kinematics to solve for positioning Peff on link @a j as close as possible to @a Ptgt (base coordinates); if solution found, stores result in this posture and returns true 00174 /*! @param Ptgt_x the target x position (relative to base frame) 00175 * @param Ptgt_y the target y position (relative to base frame) 00176 * @param Ptgt_z the target z position (relative to base frame) 00177 * @param link the output offset of the joint to move 00178 * @param Peff_x the x position (relative to @a link) which you desire to have moved to @a Ptgt (it's a point on the effector) 00179 * @param Peff_y the y position (relative to @a link) which you desire to have moved to @a Ptgt (it's a point on the effector) 00180 * @param Peff_z the z position (relative to @a link) which you desire to have moved to @a Ptgt (it's a point on the effector) 00181 * 00182 * The difference between solveLinkPosition() and solveLinkVector() is typically small, 00183 * but critical when you're trying to look at something -- the solution obtained by 00184 * simplying trying to solve for the position may not align the vector with the target -- 00185 * solveLinkVector() tries to ensure the vector is aligned with the target, even if that 00186 * isn't the closest solution position-wise. 00187 */ 00188 virtual bool solveLinkPosition(float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z) 00189 { return solveLinkPosition(fmat::pack(Ptgt_x,Ptgt_y,Ptgt_z),link,fmat::pack(Peff_x,Peff_y,Peff_z)); } 00190 00191 //! Performs inverse kinematics to solve for aligning the vector through Peff on link @a j and the link's origin to point at @a Ptgt (base coordinates in homogenous form); if solution found, stores result in this posture and returns true 00192 /*! @param Ptgt the target point, in base coordinates 00193 * @param link the output offset of the joint to move 00194 * @param Peff the point (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") 00195 * 00196 * The difference between solveLinkPosition() and solveLinkVector() is typically small, 00197 * but critical when you're trying to look at something -- the solution obtained by 00198 * simplying trying to solve for the position may not align the vector with the target -- 00199 * solveLinkVector() tries to ensure the vector is aligned with the target, even if that 00200 * isn't the closest solution position-wise. 00201 */ 00202 virtual bool solveLinkVector(const fmat::SubVector<3,const float>& Ptgt, unsigned int link, const fmat::SubVector<3,const float>& Peff); 00203 00204 //! Performs inverse kinematics to solve for aligning the vector through Peff on link @a j and the link's origin to point at @a Ptgt (base coordinates); if solution found, stores result in this posture and returns true 00205 /*! @param Ptgt_x the target x position (relative to base frame) 00206 * @param Ptgt_y the target y position (relative to base frame) 00207 * @param Ptgt_z the target z position (relative to base frame) 00208 * @param link the output offset of the joint to move 00209 * @param Peff_x the x position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") 00210 * @param Peff_y the y position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") 00211 * @param Peff_z the z position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") 00212 * 00213 * @todo this method is an approximation, could be more precise, and perhaps faster, although this is pretty good. 00214 * 00215 * The difference between solveLinkPosition() and solveLinkVector() is typically small, 00216 * but critical when you're trying to look at something -- the solution obtained by 00217 * simplying trying to solve for the position may not align the vector with the target -- 00218 * solveLinkVector() tries to ensure the vector is aligned with the target, even if that 00219 * isn't the closest solution position-wise. 00220 */ 00221 virtual bool solveLinkVector(float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z) 00222 { return solveLinkVector(pack(Ptgt_x,Ptgt_y,Ptgt_z),link,pack(Peff_x,Peff_y,Peff_z)); } 00223 00224 //! Performs inverse kinematics to solve for positioning @a Peff on link @a j as close as possible to @a Ptgt (base coordinates in homogenous form); if solution found, stores result in this posture and returns true 00225 /*! @param oriTgt the target orientation, as a quaternion relative to base frame 00226 * @param link the output offset of the joint to move 00227 * @param oriOffset an orientation offset (relative to @a link) which you desire to have moved to @a oriTgt 00228 * 00229 * The quaternion functions of fmat are helpful for creating axis-aligned rotations: fmat::quatX(r), fmat::quatY(r), fmat::quatZ(r), and fmat::QUAT_IDENTITY 00230 */ 00231 virtual bool solveLinkOrientation(const fmat::Quaternion& oriTgt, unsigned int link, const fmat::Quaternion& oriOffset); 00232 00233 //! Performs inverse kinematics to solve for both a position and orientation, bringing @a posOffset on @a link to @a posTgt, in the orientation specified. 00234 /*! @param posTgt the target point, in base coordinates 00235 * @param oriTgt the target orientation, as a quaternion relative to base frame 00236 * @param link the output offset of the joint to move 00237 * @param posOffset the point (relative to @a link) which you desire to have moved to @a posTgt (it's the desired "effector") 00238 * @param oriOffset an orientation offset (relative to @a link) which you desire to have moved to @a oriTgt 00239 * 00240 * The quaternion functions of fmat are helpful for creating axis-aligned rotations: fmat::quatX(r), fmat::quatY(r), fmat::quatZ(r), and fmat::QUAT_IDENTITY 00241 */ 00242 virtual bool solveLink(const fmat::SubVector<3,const float>& posTgt, const fmat::Quaternion& oriTgt, unsigned int link, const fmat::SubVector<3,const float>& posOffset, const fmat::Quaternion& oriOffset); 00243 00244 //@} 00245 00246 00247 00248 //!@name Combining Postures 00249 00250 //! sets joints of this to all joints of @a pe which are not equal to unused (layers @a pe over this) stores into this 00251 virtual PostureEngine& setOverlay(const PostureEngine& pe); 00252 //! sets joints of this to all joints of @a pe which are not equal to unused (layers @a pe over this) returns new PostureEngine 00253 virtual PostureEngine createOverlay(const PostureEngine& pe) const; 00254 00255 //! sets joints of this which are equal to unused to @a pe, (layers this over @a pe) stores into this 00256 virtual PostureEngine& setUnderlay(const PostureEngine& pe); 00257 //! sets joints of this which are equal to unused to @a pe, (layers this over @a pe) returns new PostureEngine 00258 virtual PostureEngine createUnderlay(const PostureEngine& pe) const; 00259 00260 //! computes a weighted average of this vs. @a pe, @a w being the weight towards @a pe (so @a w==1 just copies @a pe) 00261 virtual PostureEngine& setAverage(const PostureEngine& pe,float w=0.5); 00262 //! computes a weighted average of this vs. @a pe, @a w being the weight towards @a pe (so @a w==1 just copies @a pe) 00263 virtual PostureEngine createAverage(const PostureEngine& pe,float w=0.5) const; 00264 00265 //! computes a weighted average of this vs. @a pe, using the weight values of the joints, storing the total weight in the result's weight value 00266 virtual PostureEngine& setCombine(const PostureEngine& pe); 00267 //! computes a weighted average of this vs. @a pe, using the weight values of the joints, storing the total weight in the result's weight value 00268 virtual PostureEngine createCombine(const PostureEngine& pe) const; 00269 00270 //! returns the sum squared error between this and pe's output values, but only between outputs which are both not unused 00271 /*! @todo create a version which does weighted summing? This treats weights as all or nothing */ 00272 virtual float diff(const PostureEngine& pe) const; 00273 00274 //! returns the average sum squared error between this and pe's output values for outputs which are both not unused 00275 /*! @todo create a version which does weighted summing? This treats weights as all or nothing */ 00276 virtual float avgdiff(const PostureEngine& pe) const; 00277 00278 //! returns the max error between this and pe's output values for outputs which are both not unused 00279 /*! @todo create a version which does weighted summing? This treats weights as all or nothing */ 00280 virtual float maxdiff(const PostureEngine& pe) const; 00281 00282 //@} 00283 00284 protected: 00285 //! enumeration of the different section types that may be used as section tags in verbose mode 00286 enum section_t { 00287 SECTION_METAINFO, //!< includes timestamp and framenumber 00288 SECTION_OUTPUTS, //!< entries corresponding to NumOutputs 00289 SECTION_BUTTONS, //!< entries corresponding to NumButtons 00290 SECTION_SENSORS, //!< entries corresponding to NumSensors 00291 SECTION_PIDDUTIES //!< entries corresponding to NumPIDJoints 00292 }; 00293 //! helper function for loadBuffer, called for each individual line 00294 virtual bool loadLine(unsigned int linenum, const char* filename, const std::map<std::string,section_t>& sectionMap, std::vector<std::string>& words, section_t& curSection, const Capabilities*& caps, bool& filtered); 00295 //! helper function for loadLine, strips trailing '~'s from output names to provide backward compatability (note pass by reference, operates 'in-place' on @a word) 00296 /*! (originally, all output names were uniform length and used '~'s as padding... ugh.) */ 00297 void stripTildes(std::string& str) { 00298 str.erase(str.find_last_not_of('~')+1); // if n is npos, npos is -1U, so becomes 0... should be safe :) 00299 } 00300 00301 //! overriding Kinematics::update, make all updates come from this posture engine's own state, not WorldState 00302 virtual void update() const; 00303 00304 //!the table of outputs' values and weights, can be accessed through setOutputCmd() and getOutputCmd() 00305 OutputCmd cmds[NumOutputs]; 00306 00307 bool saveFormatCondensed; //!< requests a condensed file format, smaller but less readable 00308 WorldState* saveSensors; //!< If non-null, saves will include sensor readings from here 00309 SensorInfo* loadSensors; //!< if non-null, loads will store any included sensor information here 00310 }; 00311 00312 /*! @file 00313 * @brief Describes PostureEngine, a base class for managing the values and weights of all the outputs 00314 * @author ejt (Creator) 00315 */ 00316 00317 #endif |
Tekkotsu v5.1CVS |
Generated Mon May 9 04:58:49 2016 by Doxygen 1.6.3 |