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 /*! 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 |