Homepage Demos Overview Downloads Tutorials Reference
Credits

HeadPointerMC.h

Go 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 #include "roboop/robot.h"
00009 
00010 //! This class gives some quick and easy functions to point the head at things
00011 class HeadPointerMC : public MotionCommand {
00012 public:
00013   //! Constructor, defaults to all joints to current value in ::state
00014   HeadPointerMC();
00015   
00016   //! Destructor
00017   virtual ~HeadPointerMC() {}
00018   
00019   //!@name Speed Control
00020   
00021   //! Sets #maxSpeed to 0 (no maximum)
00022   void noMaxSpeed() { for(unsigned int i=0; i<NumHeadJoints; i++) maxSpeed[i]=0; }
00023   
00024   //! Restores #maxSpeed to default settings from Config::Motion_Config
00025   void defaultMaxSpeed();
00026   
00027   //! Sets #maxSpeed in rad/sec
00028   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t)
00029    *  @param x maximum radians per second to move */
00030   void setMaxSpeed(unsigned int i, float x) { maxSpeed[i]=x*FrameTime/1000; }
00031   
00032   //! Returns #maxSpeed in rad/sec
00033   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t)
00034    *  @return the maximum speed of joint @a i in radians per second */
00035   float getMaxSpeed(unsigned int i) { return maxSpeed[i]*1000/FrameTime; }
00036   
00037   //@}
00038   
00039   //!@name Joint Accessors
00040   
00041   //! Sets the weight values for all the neck joints
00042   void setWeight(float w);
00043   
00044   //! Directly sets the neck values (all values in radians)
00045   /*! @param j1 value for first neck joint (tilt on all ERS models)
00046    *  @param j2 value for second neck joint (pan on all ERS models)
00047    *  @param j3 value for third neck joint (nod on ERS-7, roll on ERS-2xx) */
00048   void setJoints(float j1, float j2, float j3);
00049   
00050   //! Directly set a single neck joint value
00051   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t)
00052    *  @param value the value to be assigned to join @a i, in radians */
00053   void setJointValue(unsigned int i, float value) {
00054     if(!ensureValidJoint(i))
00055       return;
00056     headTargets[i]=clipAngularRange(HeadOffset+i,value);
00057     markDirty();
00058   }
00059   
00060   //! Returns the target value of joint @a i.  Use this if you want to know the current @b commanded joint value; To get the current joint @b position, look in WorldState::outputs
00061   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) */
00062   float getJointValue(unsigned int i) const {
00063     if(ensureValidJoint(i))
00064       return headTargets[i];
00065     else
00066       return 0;
00067   }
00068   
00069   //! Centers the camera on a point in space, attempting to keep the camera as far away from the point as possible
00070   /*! Point should be relative to the body reference frame (see ::BaseFrameOffset)
00071    *  @param x location in millimeters
00072    *  @param y location in millimeters
00073    *  @param z location in millimeters
00074    *
00075    *  @todo this method is an approximation, could be more precise, and perhaps faster, although this is pretty good. */
00076   bool lookAtPoint(float x, float y, float z);
00077   
00078   //! Centers the camera on a point in space, attempting to move the camera @a d millimeters away from the point
00079   /*! Point should be relative to the body reference frame (see ::BaseFrameOffset)
00080    *  @param x location in millimeters
00081    *  @param y location in millimeters
00082    *  @param z location in millimeters
00083    *  @param d target distance from point in millimeters */
00084   bool lookAtPoint(float x, float y, float z, float d);
00085   
00086   //! Points the camera in a given direction
00087   /*! Vector should be relative to the body reference frame (see ::BaseFrameOffset)
00088    *  @param x component of the direction vector
00089    *  @param y component of the direction vector
00090    *  @param z component of the direction vector */
00091   bool lookInDirection(float x, float y, float z);
00092   
00093   //@}
00094   
00095   
00096   //!@name Inherited:
00097   virtual int updateOutputs(); //!< Updates where the head is looking
00098   virtual int isDirty() { return (dirty || !targetReached)?3:0; } //!< true if a change has been made since the last updateJointCmds() and we're active
00099   virtual int isAlive() { return true; } //!< always true, doesn't autoprune (yet)
00100   virtual void DoStart() { MotionCommand::DoStart(); markDirty(); } //!< marks this as dirty each time it is added
00101   //@}
00102 
00103  protected:
00104   //! puts x in the range (-pi,pi)
00105   static float normalizeAngle(float x) { return x-rint(x/(2*M_PI))*(2*M_PI); }
00106   
00107   //! 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
00108   static float clipAngularRange(unsigned int i, float x) {
00109     float min=outputRanges[i][MinRange];
00110     float max=outputRanges[i][MaxRange];
00111     if(x<min || x>max) {
00112       float mn_dist=fabs(normalizeAngle(min-x));
00113       float mx_dist=fabs(normalizeAngle(max-x));
00114       if(mn_dist<mx_dist)
00115         return min;
00116       else
00117         return max;
00118     } else
00119       return x;
00120   }
00121   //! if targetReached, reassigns headCmds from ::state, then sets dirty to true and targetReached to false
00122   /*! should be called each time a joint value gets modified in case
00123    *  the head isn't where it's supposed to be, it won't jerk around */
00124   void markDirty();
00125 
00126   //! Makes sure @a i is in the range (0,NumHeadJoints).  If it is instead in the range (HeadOffset,HeadOffset+NumHeadJoints), output a warning and reset @a i to the obviously intended value.
00127   /*! @param[in] i joint offset relative to either HeadOffset (i.e. one of TPROffset_t) or 0
00128    *  @param[out] i joint offset relative to HeadOffset (i.e. one of TPROffset_t)
00129    *  @return true if the intended joint could be ascertained, false otherwise */
00130   static bool ensureValidJoint(unsigned int& i);
00131 
00132   bool dirty;                          //!< true if a change has been made since last call to updateJointCmds()
00133   bool targetReached;                  //!< false if the head is still moving towards its target
00134   float headTargets[NumHeadJoints];    //!< stores the target value of each joint
00135   OutputCmd headCmds[NumHeadJoints];   //!< stores the last values we sent from updateOutputs
00136   float maxSpeed[NumHeadJoints];       //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame
00137   ROBOOP::Robot headkin;               //!< provides kinematics computations
00138 };
00139 
00140 /*! @file
00141  * @brief Describes HeadPointerMC, a class for various ways to control where the head is looking
00142  * @author ejt (Creator)
00143  *
00144  * $Author: ejt $
00145  * $Name: tekkotsu-2_2_2 $
00146  * $Revision: 1.18 $
00147  * $State: Exp $
00148  * $Date: 2004/12/23 00:59:03 $
00149  */
00150 
00151 #endif
00152 

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0