Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
OldHeadPointerMC.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_OldHeadPointerMC_h 00003 #define INCLUDED_OldHeadPointerMC_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 OldHeadPointerMC : public MotionCommand { 00011 public: 00012 //! constructor, defaults to active, BodyRelative, all joints at 0 00013 OldHeadPointerMC(); 00014 00015 //! destructor 00016 virtual ~OldHeadPointerMC() {} 00017 00018 //! 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 00019 enum CoordFrame_t { 00020 BodyRelative, //!<holds neck at a specified position, like a PostureEngine, but neck specific 00021 GravityRelative //!<uses accelerometers to keep a level head, doesn't apply for pan joint, but in future could use localization for pan 00022 }; 00023 00024 void setWeight(double w); //!< sets the weight values for all the neck joints 00025 00026 //! set a specific head joint weight, pass one of RobotInfo::TPROffset_t, not a full offset! 00027 void setWeight(RobotInfo::TPROffset_t i, double weight) { 00028 dirty=true; targetReached=false; headCmds[i].weight=weight; } 00029 00030 void setActive(bool a) { active=a; } //!< sets #active flag; see isDirty() 00031 bool getActive() const { return active; } //!< returns #active flag, see isDirty() 00032 00033 //! sets #maxSpeed to 0 (no maximum) 00034 void noMaxSpeed() { for(unsigned int i=0; i<NumHeadJoints; i++) maxSpeed[i]=0; } 00035 00036 void defaultMaxSpeed(); //!< restores #maxSpeed to default settings from Config::Motion_Config 00037 00038 void setMaxSpeed(TPROffset_t i, float x) { maxSpeed[i]=x*FrameTime/1000; } //!< sets #maxSpeed in rad/sec 00039 float getMaxSpeed(TPROffset_t i) { return maxSpeed[i]*1000/FrameTime; } //!< returns #maxSpeed in rad/sec 00040 00041 //! 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) 00042 double convert(RobotInfo::TPROffset_t i, double v, CoordFrame_t srcmode, CoordFrame_t tgtmode) const { 00043 return (srcmode==tgtmode)?v:convFromBodyRelative(i,convToBodyRelative(i, v, srcmode),tgtmode); 00044 } 00045 00046 //!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() 00047 //!@name Joint Accessors 00048 00049 //! Directly sets the neck values (radians), uses current mode 00050 void setJoints(double tilt, double pan, double roll); 00051 00052 //! sets all the joints to the given mode, will convert the values to the new mode if @a convert is true 00053 void setMode(CoordFrame_t m, bool convert=true); 00054 00055 //! sets 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! 00056 void setJointMode(RobotInfo::TPROffset_t i, CoordFrame_t m, bool convert=true); 00057 00058 //! set a specific head joint's value (in radians, for whatever mode it's in), pass one of RobotInfo::TPROffset_t, not a full offset! 00059 void setJointValue(RobotInfo::TPROffset_t i, double value) 00060 { dirty=true; targetReached=false; headTargets[i]=(headModes[i]==BodyRelative)?clipAngularRange(i+HeadOffset,value):value; } 00061 00062 //! set a specific head joint (in radians), pass one of RobotInfo::TPROffset_t, not a full offset! 00063 void setJointValueAndMode(RobotInfo::TPROffset_t i, double value, CoordFrame_t m) 00064 { dirty=true; targetReached=false; headTargets[i]=(m==BodyRelative)?clipAngularRange(i+HeadOffset,value):value; headModes[i]=m; } 00065 00066 //! set a specific head joint (in radians) auto converting @a value from mode @a m to the current mode, pass one of RobotInfo::TPROffset_t, not a full offset! 00067 void setJointValueFromMode(RobotInfo::TPROffset_t i, double value, CoordFrame_t m) 00068 { dirty=true; targetReached=false; headTargets[i]=convert(i,value,m,headModes[i]); } 00069 00070 //! returns the current mode for joint @a i (use RobotInfo::TPROffset_t, not global offset) 00071 CoordFrame_t getJointMode(RobotInfo::TPROffset_t i) const { return headModes[i]; } 00072 00073 //! returns the target value (relative to the current mode) of joint @a i. Use getOutputCmd() if you want to know the current @b commanded joint value; To get the current joint @b position, look in WorldState 00074 double getJointValue(RobotInfo::TPROffset_t i) const { return headTargets[i]; } 00075 //@} 00076 00077 00078 //!@name Inherited: 00079 virtual int updateOutputs(); //!< Updates where the head is looking 00080 virtual const OutputCmd& getOutputCmd(unsigned int i); //!< returns one of the #headJoints entries or ::unusedJoint if not a head joint 00081 virtual int isDirty() { return ((dirty || !targetReached) && active)?1:0; } //!< true if a change has been made since the last updateJointCmds() and we're active 00082 virtual int isAlive() { return true; } 00083 virtual void DoStart() { MotionCommand::DoStart(); dirty=true; targetReached=false; } 00084 //@} 00085 00086 protected: 00087 double convToBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const; //!< converts to a body relative measurement for joint @a i 00088 double convFromBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const; //!< converts from a body relative measurement for joint @a i 00089 static float normalizeAngle(float x) { return x-rint(x/(2*M_PI))*(2*M_PI); } //!< puts x in the range (-pi,pi) 00090 //! if @a x is outside of the range of joint @a i, it is set to either the min or the max, whichever is closer 00091 static float clipAngularRange(unsigned int i, float x) { 00092 float min=outputRanges[i][MinRange]; 00093 float max=outputRanges[i][MaxRange]; 00094 if(x<min || x>max) { 00095 float mn_dist=fabs(normalizeAngle(min-x)); 00096 float mx_dist=fabs(normalizeAngle(max-x)); 00097 if(mn_dist<mx_dist) 00098 return min; 00099 else 00100 return max; 00101 } else 00102 return x; 00103 } 00104 00105 bool dirty; //!< true if a change has been made since last call to updateJointCmds() 00106 bool active; //!< set by accessor functions, defaults to true 00107 bool targetReached; //!< false if the head is still moving towards its target 00108 OutputCmd headCmds[NumHeadJoints] ; //!< stores the last values we sent from updateOutputs 00109 float headTargets[NumHeadJoints]; //!< stores the target value of each joint, relative to #headModes 00110 CoordFrame_t headModes[NumHeadJoints]; //!< stores the current mode of each joint, for instance so tilt can be GravityRelative while pan is static 00111 float maxSpeed[NumHeadJoints]; //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame 00112 }; 00113 00114 /*! @file 00115 * @brief Describes OldHeadPointerMC, a class for various ways to control where the head is looking 00116 * @author ejt (Creator) 00117 * 00118 * $Author: ejt $ 00119 * $Name: tekkotsu-2_2 $ 00120 * $Revision: 1.1 $ 00121 * $State: Exp $ 00122 * $Date: 2004/10/14 20:23:50 $ 00123 */ 00124 00125 #endif 00126 |
Tekkotsu v2.2 |
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1 |