Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PostureEngine.h

Go 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&nbsp;.*</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>&lt;</tt><i>section</i><tt>&gt;</tt> ... <tt>&lt;/</tt><i>section</i><tt>&gt;</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