Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

PostureEngine.h Go to the documentation of this file. //-*-c++-*- #ifndef INCLUDED_PostureEngine_h #define INCLUDED_PostureEngine_h #include "Motion/OutputCmd.h" #include "Motion/Kinematics.h" #include "Shared/RobotInfo.h" #include "Shared/LoadSave.h" class WorldState; //! A class for storing a set of positions and weights for all the outputs /*! Handy for any class which wants to deal with setting joints and postures without writing a custom class * @see PostureMC * @see <a href="http://www.cs.cmu.edu/~tekkotsu/Kinematics.html">Tekkotsu's Kinematics page</a> * @see <a href="http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/postures.shtml">David Touretzky's "Postures and Motion Sequences" Chapter</a> * @see <a href="http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/forwardkin.shtml">David Touretzky's "Forward Kinematics" Chapter</a> * @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> * @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> */ class PostureEngine : public LoadSave, public Kinematics { public: //!@name Constructors //!constructor PostureEngine() : LoadSave(), Kinematics(*kine), saveFormatCondensed(false), loadSaveSensors(NULL) {} //!constructor, loads a position from a file /*! @todo might want to make a library stored in memory of common positions so they don't have to be loaded repeatedly from memstick */ PostureEngine(const std::string& filename) : LoadSave(), Kinematics(*kine), saveFormatCondensed(false), loadSaveSensors(NULL) { loadFile(filename.c_str()); } //!constructor, initializes joint positions to the current state of the outputs as defined by @a state PostureEngine(const WorldState* st) : LoadSave(), Kinematics(*kine), saveFormatCondensed(false), loadSaveSensors(NULL) { if(st!=NULL) takeSnapshot(*st); } //! copy constructor PostureEngine(const PostureEngine& pe) : LoadSave(pe), Kinematics(pe), saveFormatCondensed(pe.saveFormatCondensed), loadSaveSensors(pe.loadSaveSensors) { for(unsigned int i=0; i<NumOutputs; i++) cmds[i]=pe.cmds[i]; } //! assignment operator PostureEngine& operator=(const PostureEngine& pe) { LoadSave::operator=(pe); Kinematics::operator=(pe); saveFormatCondensed=pe.saveFormatCondensed; loadSaveSensors=pe.loadSaveSensors; for(unsigned int i=0; i<NumOutputs; i++) cmds[i]=pe.cmds[i]; return *this; } //! destructor virtual ~PostureEngine(); //@} //! You should be able to call the non-virtual functions without checking out, just a MotionManager::peekMotion(). Theoretically. //!@name Output Value Access/Control virtual void takeSnapshot(); //!< sets the values of #cmds to the current state of the outputs (doesn't change the weights) 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) virtual void setWeights(float w) { setWeights(w,0,NumOutputs); } //!< set the weights of all #cmds virtual void setWeights(float w, unsigned int lowjoint, unsigned int highjoint); //!< the the weights of a range of #cmds virtual void clear(); //!< sets all joints to unused 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) 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) inline const OutputCmd& operator()(unsigned int i) const { return cmds[i]; } //!< returns output @a i inline OutputCmd& getOutputCmd(unsigned int i) { return cmds[i]; } //!< returns output @a i, returns a reference so you can also set through an assignment inline const OutputCmd& getOutputCmd(unsigned int i) const { return cmds[i]; } //!< returns output @a i //@} //!Uses LoadSave interface so you can load/save to files, uses a human-readable storage format //!@name LoadSave virtual void setSaveFormat(bool condensed, WorldState* ws); //!< sets #saveFormatCondensed and #loadSaveSensors (pass ::state for @a ws if you want to use current sensor values) virtual void setLoadedSensors(WorldState* ws) { loadSaveSensors=ws; } //!< if @a ws is non-NULL, any sensor values in loaded postures will be stored there (otherwise they are ignored) virtual WorldState* getLoadedSensors() const { return loadSaveSensors; } //!< returns value previously stored by setLoadSensors() virtual unsigned int getBinSize() const; virtual unsigned int loadBuffer(const char buf[], unsigned int len); virtual unsigned int saveBuffer(char buf[], unsigned int len) const; virtual unsigned int loadFile(const char filename[]); virtual unsigned int saveFile(const char filename[]) const; //@} //!@name Kinematics //! 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 /*! @param Ptgt the target point, in base coordinates * @param link the output offset of the joint to move * @param Peff the point (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") * * The difference between solveLinkPosition() and solveLinkVector() is typically small, * but critical when you're trying to look at something -- the solution obtained by * simplying trying to solve for the position may not align the vector with the target -- * solveLinkVector() tries to ensure the vector is aligned with the target, even if that * isn't the closest solution position-wise. */ virtual bool solveLinkPosition(const NEWMAT::ColumnVector& Ptgt, unsigned int link, const NEWMAT::ColumnVector& Peff); //! 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 /*! @param Ptgt_x the target x position (relative to base frame) * @param Ptgt_y the target y position (relative to base frame) * @param Ptgt_z the target z position (relative to base frame) * @param link the output offset of the joint to move * @param Peff_x the x position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") * @param Peff_y the y position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") * @param Peff_z the z position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") * * The difference between solveLinkPosition() and solveLinkVector() is typically small, * but critical when you're trying to look at something -- the solution obtained by * simplying trying to solve for the position may not align the vector with the target -- * solveLinkVector() tries to ensure the vector is aligned with the target, even if that * isn't the closest solution position-wise. */ virtual bool solveLinkPosition(float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z) { return solveLinkPosition(pack(Ptgt_x,Ptgt_y,Ptgt_z),link,pack(Peff_x,Peff_y,Peff_z)); } //! 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 /*! @param Ptgt the target point, in base coordinates * @param link the output offset of the joint to move * @param Peff the point (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") * * The difference between solveLinkPosition() and solveLinkVector() is typically small, * but critical when you're trying to look at something -- the solution obtained by * simplying trying to solve for the position may not align the vector with the target -- * solveLinkVector() tries to ensure the vector is aligned with the target, even if that * isn't the closest solution position-wise. */ virtual bool solveLinkVector(const NEWMAT::ColumnVector& Ptgt, unsigned int link, const NEWMAT::ColumnVector& Peff); //! 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 /*! @param Ptgt_x the target x position (relative to base frame) * @param Ptgt_y the target y position (relative to base frame) * @param Ptgt_z the target z position (relative to base frame) * @param link the output offset of the joint to move * @param Peff_x the x position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") * @param Peff_y the y position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") * @param Peff_z the z position (relative to @a link) which you desire to have moved to @a Ptgt (it's the desired "effector") * * @todo this method is an approximation, could be more precise, and perhaps faster, although this is pretty good. * * The difference between solveLinkPosition() and solveLinkVector() is typically small, * but critical when you're trying to look at something -- the solution obtained by * simplying trying to solve for the position may not align the vector with the target -- * solveLinkVector() tries to ensure the vector is aligned with the target, even if that * isn't the closest solution position-wise. */ virtual bool solveLinkVector(float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z) { return solveLinkVector(pack(Ptgt_x,Ptgt_y,Ptgt_z),link,pack(Peff_x,Peff_y,Peff_z)); } //@} //!@name Combining Postures //! sets joints of this to all joints of @a pe which are not equal to unused (layers @a pe over this) stores into this virtual PostureEngine& setOverlay(const PostureEngine& pe); //! sets joints of this to all joints of @a pe which are not equal to unused (layers @a pe over this) returns new PostureEngine virtual PostureEngine createOverlay(const PostureEngine& pe) const; //! sets joints of this which are equal to unused to @a pe, (layers this over @a pe) stores into this virtual PostureEngine& setUnderlay(const PostureEngine& pe); //! sets joints of this which are equal to unused to @a pe, (layers this over @a pe) returns new PostureEngine virtual PostureEngine createUnderlay(const PostureEngine& pe) const; //! 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) virtual PostureEngine& setAverage(const PostureEngine& pe,float w=0.5); //! 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) virtual PostureEngine createAverage(const PostureEngine& pe,float w=0.5) const; //! 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 virtual PostureEngine& setCombine(const PostureEngine& pe); //! 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 virtual PostureEngine createCombine(const PostureEngine& pe) const; //! returns the sum squared error between this and pe's output values, but only between outputs which are both not unused /*! @todo create a version which does weighted summing? This treats weights as all or nothing */ virtual float diff(const PostureEngine& pe) const; //! returns the average sum squared error between this and pe's output values for outputs which are both not unused /*! @todo create a version which does weighted summing? This treats weights as all or nothing */ virtual float avgdiff(const PostureEngine& pe) const; //! returns the max error between this and pe's output values for outputs which are both not unused /*! @todo create a version which does weighted summing? This treats weights as all or nothing */ virtual float maxdiff(const PostureEngine& pe) const; //@} protected: //all updates come from this posture engine's own state, not WorldState virtual void update(unsigned int c, unsigned int l); //!the table of outputs' values and weights, can be accessed through setOutputCmd() and getOutputCmd() OutputCmd cmds[NumOutputs]; bool saveFormatCondensed; //!< requests a condensed file format, smaller but less readable WorldState* loadSaveSensors; //!< If non-null, saves will include sensor readings from here, and loads will store any read sensors into here }; /*! @file * @brief Describes PostureEngine, a base class for managing the values and weights of all the outputs * @author ejt (Creator) * * $Author: ejt $ * $Name: tekkotsu-3_0 $ * $Revision: 1.23 $ * $State: Exp $ * $Date: 2006/10/04 02:57:12 $ */ #endif

Tekkotsu v3.0

Generated Wed Oct 4 00:03:45 2006 by Doxygen 1.4.7