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) { headTargets[i]=clipAngularRange(HeadOffset+i,value);  markDirty(); }
00054   
00055   //! 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
00056   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) */
00057   float getJointValue(unsigned int i) const { return headTargets[i]; }
00058   
00059   //! Centers the camera on a point in space, attempting to keep the camera as far away from the point as possible
00060   /*! @param x location in millimeters
00061    *  @param y location in millimeters
00062    *  @param z location in millimeters */
00063   bool lookAtPoint(float x, float y, float z);
00064   
00065   //! Centers the camera on a point in space, attempting to move the camera @a d millimeters away from the point
00066   /*! @param x location in millimeters
00067    *  @param y location in millimeters
00068    *  @param z location in millimeters
00069    *  @param d target distance from point in millimeters */
00070   bool lookAtPoint(float x, float y, float z, float d);
00071   
00072   //! Points the camera in a given direction
00073   /*! @param x component of the direction vector
00074    *  @param y component of the direction vector
00075    *  @param z component of the direction vector */
00076   bool lookInDirection(float x, float y, float z);
00077   
00078   //@}
00079   
00080   
00081   //!@name Inherited:
00082   virtual int updateOutputs(); //!< Updates where the head is looking
00083   virtual int isDirty() { return (dirty || !targetReached)?3:0; } //!< true if a change has been made since the last updateJointCmds() and we're active
00084   virtual int isAlive() { return true; } //!< always true, doesn't autoprune (yet)
00085   virtual void DoStart() { MotionCommand::DoStart(); markDirty(); } //!< marks this as dirty each time it is added
00086   //@}
00087 
00088  protected:
00089   //! puts x in the range (-pi,pi)
00090   static float normalizeAngle(float x) { return x-rint(x/(2*M_PI))*(2*M_PI); }
00091   
00092   //! 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
00093   static float clipAngularRange(unsigned int i, float x) {
00094     float min=outputRanges[i][MinRange];
00095     float max=outputRanges[i][MaxRange];
00096     if(x<min || x>max) {
00097       float mn_dist=fabs(normalizeAngle(min-x));
00098       float mx_dist=fabs(normalizeAngle(max-x));
00099       if(mn_dist<mx_dist)
00100         return min;
00101       else
00102         return max;
00103     } else
00104       return x;
00105   }
00106   //! if targetReached, reassigns headCmds from ::state, then sets dirty to true and targetReached to false
00107   /*! should be called each time a joint value gets modified in case
00108    *  the head isn't where it's supposed to be, it won't jerk around */
00109   void markDirty();
00110 
00111   bool dirty;                          //!< true if a change has been made since last call to updateJointCmds()
00112   bool targetReached;                  //!< false if the head is still moving towards its target
00113   float headTargets[NumHeadJoints];    //!< stores the target value of each joint
00114   OutputCmd headCmds[NumHeadJoints];   //!< stores the last values we sent from updateOutputs
00115   float maxSpeed[NumHeadJoints];       //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame
00116   ROBOOP::Robot headkin;               //!< provides kinematics computations
00117 };
00118 
00119 /*! @file
00120  * @brief Describes HeadPointerMC, a class for various ways to control where the head is looking
00121  * @author ejt (Creator)
00122  *
00123  * $Author: ejt $
00124  * $Name: tekkotsu-2_2_1 $
00125  * $Revision: 1.14 $
00126  * $State: Exp $
00127  * $Date: 2004/10/14 23:02:53 $
00128  */
00129 
00130 #endif
00131 

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:38 2004 by Doxygen 1.3.9.1