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
00071   inline OutputCmd& operator()(unsigned int i) { return cmds[i]; } //!< returns output @a i, returns a reference so you can also set "through" this call.
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" this call.
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 Plink on link @a j to @a Pobj in base coordinates (expects homogenous form); if solution found, stores result in this posture and returns true
00087   bool solveLinkPosition(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink);
00088   //! Performs inverse kinematics to solve for positioning Plink on link @a j to Pobj in base coordinates; if solution found, stores result in this posture and returns true
00089   bool solveLinkPosition(float Pobj_x, float Pobj_y, float Pobj_z, unsigned int j, float Plink_x, float Plink_y, float Plink_z)
00090   { return solveLinkPosition(pack(Pobj_x,Pobj_y,Pobj_z),j,pack(Plink_x,Plink_y,Plink_z)); }
00091 
00092   //! Performs inverse kinematics to solve for positioning @a Plink on link @a j to @a Pobj in base coordinates (expects homogenous form); if solution found, stores result in this posture and returns true
00093   bool solveLinkVector(const NEWMAT::ColumnVector& Pobj, unsigned int j, const NEWMAT::ColumnVector& Plink);
00094   //! Performs inverse kinematics to solve for positioning Plink on link @a j to Pobj in base coordinates; if solution found, stores result in this posture and returns true
00095   bool solveLinkVector(float Pobj_x, float Pobj_y, float Pobj_z, unsigned int j, float Plink_x, float Plink_y, float Plink_z)
00096   { return solveLinkVector(pack(Pobj_x,Pobj_y,Pobj_z),j,pack(Plink_x,Plink_y,Plink_z)); }
00097 
00098 protected:
00099   //all updates come from this posture engine's own state, not WorldState
00100   virtual void update(unsigned int c, unsigned int l);
00101 
00102   //!the table of outputs' values and weights, can be accessed through setOutputCmd() and getOutputCmd()
00103   OutputCmd cmds[NumOutputs];
00104 };
00105 
00106 /*! @file
00107  * @brief Describes PostureEngine, a base class for managing the values and weights of all the outputs
00108  * @todo write a binary version of Load/Save commands for faster access
00109  * @author ejt (Creator)
00110  *
00111  * $Author: ejt $
00112  * $Name: tekkotsu-2_2 $
00113  * $Revision: 1.13 $
00114  * $State: Exp $
00115  * $Date: 2004/10/18 19:53:33 $
00116  */
00117 
00118 #endif

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1