Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

OldHeadPointerMC.h

Go 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 #headCmds 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_4_1 $
00120  * $Revision: 1.2 $
00121  * $State: Exp $
00122  * $Date: 2005/08/07 04:11:03 $
00123  */
00124 
00125 #endif
00126 

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