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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4