Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
HeadPointerMC.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_HeadPointerMC_h 00003 #define INCLUDED_HeadPointerMC_h 00004 00005 #include "MotionCommand.h" 00006 #include "OutputCmd.h" 00007 #include "Shared/RobotInfo.h" 00008 00009 //! This class gives some quick and easy functions to point the head at things 00010 class HeadPointerMC : public MotionCommand { 00011 public: 00012 //! constructor, defaults to active, BodyRelative, all joints at 0 00013 HeadPointerMC(); 00014 //! destructor 00015 virtual ~HeadPointerMC() {} 00016 00017 //! Various modes the head can be in. In the future may want to add ability to explicitly track an object or point in the world model 00018 enum CoordFrame_t { 00019 BodyRelative, //!<holds neck at a specified position, like a PostureEngine, but neck specific 00020 GravityRelative //!<uses accelerometers to keep a level head, doesn't apply for pan joint, but in future could use localization for pan 00021 }; 00022 00023 void setWeight(double w); //!< sets the weight values for all the neck joints 00024 inline void setWeight(RobotInfo::TPROffset_t i, double weight) { dirty=true; headJoints[i].weight=weight; } //!< set a specific head joint weight, pass one of RobotInfo::TPROffset_t, not a full offset! 00025 inline void setActive(bool a) { active=a; } //!< sets #active flag, see isDirty() 00026 inline bool getActive() const { return active; } //!< returns #active flag, see isDirty() 00027 00028 void noMaxSpeed() { for(unsigned int i=0; i<NumHeadJoints; i++) maxSpeed[i]=0; } //!< sets #maxSpeed to 0 (no maximum) 00029 void defaultMaxSpeed(); //!< restores #maxSpeed to default settings from Config::Motion_Config 00030 void setMaxSpeed(TPROffset_t i, float x) { maxSpeed[i]=x*FrameTime/1000; } //!< sets #maxSpeed, pass it rad/sec 00031 float getMaxSpeed(TPROffset_t i) { return maxSpeed[i]*1000/FrameTime; } //!< returns #maxSpeed in rad/sec 00032 00033 //! converts a value @a v in @a srcmode to a value in @a tgtmode that would leave the joint angle for joint @a i constant (you probably won't need to call this directly) 00034 inline double convert(RobotInfo::TPROffset_t i, double v, CoordFrame_t srcmode, CoordFrame_t tgtmode) const { 00035 return (srcmode==tgtmode)?v:convFromBodyRelative(i,convToBodyRelative(i, v, srcmode),tgtmode); 00036 } 00037 00038 //!Note that none of these are @c virtual, so you don't have to checkout to use them, you @e should be able to use MotionManager::peekMotion() 00039 //!@name Joint Accessors 00040 void setJoints(double tilt, double pan, double roll); //!< Directly sets the neck values, uses current mode 00041 void setMode(CoordFrame_t m, bool convert=true); //!< sets all the joints to the given mode, will convert the values to the new mode if @a convert is true 00042 void setJointMode(RobotInfo::TPROffset_t i, CoordFrame_t m, bool convert=true); //!< set a specific head joint's mode, will convert from previous mode's value to next mode's value if convert is true. Pass one of RobotInfo::TPROffset_t, not a full offset! 00043 inline void setJointValue(RobotInfo::TPROffset_t i, double value) { dirty=true; headValues[i]=value;} //!< set a specific head joint's value (for whatever mode it's in), pass one of RobotInfo::TPROffset_t, not a full offset! 00044 inline void setJointValueAndMode(RobotInfo::TPROffset_t i, double value, CoordFrame_t m) { dirty=true; headValues[i]=value; headModes[i]=m; } //!< set a specific head joint, pass one of RobotInfo::TPROffset_t, not a full offset! 00045 inline void setJointValueFromMode(RobotInfo::TPROffset_t i, double value, CoordFrame_t m) { dirty=true; headValues[i]=convert(i,value,m,headModes[i]); } //!< set a specific head joint auto converting @a value from mode @a m to the current mode, pass one of RobotInfo::TPROffset_t, not a full offset! 00046 inline CoordFrame_t getJointMode(RobotInfo::TPROffset_t i) const { return headModes[i]; } //!< returns the current mode for joint @a i (use RobotInfo::TPROffset_t, not global offset) 00047 inline double getJointValue(RobotInfo::TPROffset_t i) const { return headValues[i]; } //!< returns the current value (relative to the current mode) of joint @a i, use getOutputCmd() if you want to know the actual @b target joint value (to get the actual current joint position, look in WorldState 00048 //@} 00049 00050 //!@name Inherited: 00051 virtual int updateOutputs( ); //!< Updates where the head is looking 00052 virtual const OutputCmd& getOutputCmd(unsigned int i); //!< returns one of the #headJoints entries or ::unusedJoint if not a head joint 00053 virtual int isDirty() { return (dirty && active)?1:0; } //!< true if a change has been made since the last updateJointCmds() and we're active 00054 virtual int isAlive() { return true; } 00055 //@} 00056 00057 protected: 00058 double convToBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const; //!< converts to a body relative measurement for joint @a i 00059 double convFromBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const; //!< converts from a body relative measurement for joint @a i 00060 00061 bool dirty; //!< true if a change has been made since last call to updateJointCmds() 00062 bool active; //!< set by accessor functions, defaults to true 00063 OutputCmd headJoints[NumHeadJoints]; //!< stores the last values we sent from updateOutputs 00064 float headValues[NumHeadJoints]; //!< stores the target value of each joint, relative to #headModes 00065 CoordFrame_t headModes[NumHeadJoints]; //!< stores the current mode of each joint, for instance so tilt can be GravityRelative while pan is static 00066 00067 float maxSpeed[NumHeadJoints]; //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame 00068 }; 00069 00070 /*! @file 00071 * @brief Describes HeadPointerMC, a class for various ways to control where the head is looking 00072 * @author ejt (Creator) 00073 * 00074 * $Author: ejt $ 00075 * $Name: tekkotsu-2_1 $ 00076 * $Revision: 1.7 $ 00077 * $State: Rel $ 00078 * $Date: 2003/09/07 22:14:01 $ 00079 */ 00080 00081 #endif 00082 |
Tekkotsu v2.1 |
Generated Tue Mar 16 23:19:13 2004 by Doxygen 1.3.5 |