Homepage Demos Overview Downloads Tutorials 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 /*! Handy for any class which wants to deal with setting joints and postures without writing a custom class
00014  *  @see PostureMC */
00015 class PostureEngine : public LoadSave, public Kinematics {
00016 public:
00017   //!constructor
00018   PostureEngine() : LoadSave(), Kinematics(*kine) {}
00019   //!constructor, loads a position from a file
00020   /*! @todo might want to make a library of common positions so they don't have to be loaded repeatedly from memstick */
00021   PostureEngine(const char * filename) : LoadSave(), Kinematics(*kine) { LoadFile(filename); }
00022   //!constructor, initializes joint positions to the current state of the outputs as defined by @a state
00023   PostureEngine(const WorldState* st) : LoadSave(), Kinematics(*kine) { takeSnapshot(st); }
00024   //! destructor
00025   virtual ~PostureEngine();
00026 
00027   //! sets the internal #cmds to the current state of the outputs
00028   virtual void takeSnapshot();
00029 
00030   //! sets the internal #cmds to the current state of the outputs as defined by @a state
00031   virtual void takeSnapshot(const WorldState* st);
00032 
00033   //! sets all joints to unused
00034   virtual void clear();
00035 
00036   //! sets joints of this to all joints of @a pe which are not equal to unused (layers @a pe over this) stores into this
00037   virtual PostureEngine& setOverlay(const PostureEngine& pe);
00038   //! sets joints of this to all joints of @a pe which are not equal to unused (layers @a pe over this) returns new PostureEngine
00039   virtual PostureEngine createOverlay(const PostureEngine& pe) const;
00040 
00041   //! sets joints of this which are equal to unused to @a pe, (layers this over @a pe) stores into this
00042   virtual PostureEngine& setUnderlay(const PostureEngine& pe);
00043   //! sets joints of this which are equal to unused to @a pe, (layers this over @a pe) returns new PostureEngine
00044   virtual PostureEngine createUnderlay(const PostureEngine& pe) const;
00045 
00046   //! 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)
00047   virtual PostureEngine& setAverage(const PostureEngine& pe,float w=0.5);
00048   //! 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)
00049   virtual PostureEngine createAverage(const PostureEngine& pe,float w=0.5) const;
00050 
00051   //! 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
00052   virtual PostureEngine& setCombine(const PostureEngine& pe);
00053   //! 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
00054   virtual PostureEngine createCombine(const PostureEngine& pe) const;
00055 
00056   //! returns the sum squared error between this and pe's output values, but only between outputs which are both not unused
00057   /*! @todo create a version which does weighted summing?  This treats weights as all or nothing */
00058   float diff(const PostureEngine& pe) const;
00059   
00060   //! returns the average sum squared error between this and pe's output values for outputs which are both not unused
00061   /*! @todo create a version which does weighted summing?  This treats weights as all or nothing */
00062   float avgdiff(const PostureEngine& pe) const;
00063   
00064   //! returns the max error between this and pe's output values for outputs which are both not unused
00065   /*! @todo create a version which does weighted summing?  This treats weights as all or nothing */
00066   float maxdiff(const PostureEngine& pe) const;
00067   
00068   //! NOT VIRTUAL! You should be able to call this to set outputs without checking out, just a peekMotion().  Theoretically.
00069   //!@name Output Accessors
00070   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)
00071   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)
00072   inline const OutputCmd& operator()(unsigned int i) const { return cmds[i]; } //!< returns output @a i
00073   inline OutputCmd& getOutputCmd(unsigned int i) { return cmds[i]; } //!< returns output @a i, returns a reference so you can also set through an assignment
00074   inline const OutputCmd& getOutputCmd(unsigned int i) const { return cmds[i]; } //!< returns output @a i
00075   //@}
00076 
00077   //!Uses LoadSave interface so you can load/save to files, uses a human-readable storage format
00078   //!@name LoadSave
00079   virtual unsigned int getBinSize() const;
00080   virtual unsigned int LoadBuffer(const char buf[], unsigned int len);
00081   virtual unsigned int SaveBuffer(char buf[], unsigned int len) const;
00082   virtual unsigned int LoadFile(const char filename[]);
00083   virtual unsigned int SaveFile(const char filename[]) const;
00084   //@}
00085 
00086   //! Performs inverse kinematics to solve for positioning @a Peff on link @a j to @a Ptgt (expects homogenous form); if solution found, stores result in this posture and returns true
00087   /*! @param Ptgt the target point, in base coordinates
00088    *  @param link the output offset of the joint to move
00089    *  @param Peff the point (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") */
00090   bool solveLinkPosition(const NEWMAT::ColumnVector& Ptgt, unsigned int link, const NEWMAT::ColumnVector& Peff);
00091 
00092   //! Performs inverse kinematics to solve for positioning Peff on link @a j to @a Ptgt; if solution found, stores result in this posture and returns true
00093   /*! @param Ptgt_x the target x position (relative to base frame)
00094    *  @param Ptgt_y the target y position (relative to base frame)
00095    *  @param Ptgt_z the target z position (relative to base frame)
00096    *  @param link the output offset of the joint to move
00097    *  @param Peff_x the x position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector")
00098    *  @param Peff_y the y position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector")
00099    *  @param Peff_z the z position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") */
00100   bool solveLinkPosition(float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z)
00101   { return solveLinkPosition(pack(Ptgt_x,Ptgt_y,Ptgt_z),link,pack(Peff_x,Peff_y,Peff_z)); }
00102 
00103   //! Performs inverse kinematics to solve for positioning Peff on link @a j to point at Ptgt (expects homogenous form); if solution found, stores result in this posture and returns true
00104   /*! @param Ptgt the target point, in base coordinates
00105    *  @param link the output offset of the joint to move
00106    *  @param Peff the point (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") */
00107   bool solveLinkVector(const NEWMAT::ColumnVector& Ptgt, unsigned int link, const NEWMAT::ColumnVector& Peff);
00108 
00109   //! Performs inverse kinematics to solve for positioning @a Peff on link @a j to point at @a Ptgt; if solution found, stores result in this posture and returns true
00110   /*! @param Ptgt_x the target x position (relative to base frame)
00111    *  @param Ptgt_y the target y position (relative to base frame)
00112    *  @param Ptgt_z the target z position (relative to base frame)
00113    *  @param link the output offset of the joint to move
00114    *  @param Peff_x the x position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector")
00115    *  @param Peff_y the y position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector")
00116    *  @param Peff_z the z position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector")
00117    *
00118    *  @todo this method is an approximation, could be more precise, and perhaps faster, although this is pretty good. */
00119   bool solveLinkVector(float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z)
00120   { return solveLinkVector(pack(Ptgt_x,Ptgt_y,Ptgt_z),link,pack(Peff_x,Peff_y,Peff_z)); }
00121 
00122 protected:
00123   //all updates come from this posture engine's own state, not WorldState
00124   virtual void update(unsigned int c, unsigned int l);
00125 
00126   //!the table of outputs' values and weights, can be accessed through setOutputCmd() and getOutputCmd()
00127   OutputCmd cmds[NumOutputs];
00128 };
00129 
00130 /*! @file
00131  * @brief Describes PostureEngine, a base class for managing the values and weights of all the outputs
00132  * @todo write a binary version of Load/Save commands for faster access
00133  * @author ejt (Creator)
00134  *
00135  * $Author: ejt $
00136  * $Name: tekkotsu-2_2_2 $
00137  * $Revision: 1.15 $
00138  * $State: Exp $
00139  * $Date: 2004/12/23 00:59:03 $
00140  */
00141 
00142 #endif

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0