Homepage
Demos
Overview
Downloads
Dev. Resources
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   //!Sets #hold - if this is set to false, it will allow a persistent motion to behave the same as a pruned motion, without being pruned
00020   virtual void setHold(bool h=true) { hold=h; }
00021   virtual bool getHold() { return hold; } //!< return #hold
00022 
00023   virtual void setTolerance(float t) { tolerance=t; } //!< sets #tolerance
00024   virtual float getTolerance() { return tolerance; } //!< returns #tolerance
00025   virtual void setTimeout(unsigned int delay) { timeout=delay; } //!< sets #timeout
00026   virtual unsigned int getTimeout() { return timeout; } //!< returns #timeout
00027 
00028   //!@name Speed Control
00029   
00030   //! Sets #maxSpeed to 0 (no maximum)
00031   void noMaxSpeed() { for(unsigned int i=0; i<NumHeadJoints; i++) maxSpeed[i]=0; }
00032   
00033   //! Restores #maxSpeed to default settings from Config::Motion_Config
00034   /*! @param x ratio of the max speed to use; so 0.5 would limit motion to half the recommended upper limit */
00035   void defaultMaxSpeed(float x=1);
00036   
00037   //! Sets #maxSpeed in rad/sec
00038   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t)
00039    *  @param x maximum radians per second to move */
00040   void setMaxSpeed(unsigned int i, float x) { maxSpeed[i]=x*FrameTime/1000; }
00041   
00042   //! Returns #maxSpeed in rad/sec
00043   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t)
00044    *  @return the maximum speed of joint @a i in radians per second */
00045   float getMaxSpeed(unsigned int i) { return maxSpeed[i]*1000/FrameTime; }
00046   
00047   //@}
00048   
00049   //!@name Joint Accessors
00050   
00051   //! Sets the weight values for all the neck joints
00052   void setWeight(float w);
00053   
00054   //! Directly sets the neck values (all values in radians)
00055   /*! @param j1 value for first neck joint (tilt on all ERS models)
00056    *  @param j2 value for second neck joint (pan on all ERS models)
00057    *  @param j3 value for third neck joint (nod on ERS-7, roll on ERS-2xx) */
00058   void setJoints(float j1, float j2, float j3);
00059   
00060   //! Directly set a single neck joint value
00061   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t)
00062    *  @param value the value to be assigned to join @a i, in radians */
00063   void setJointValue(unsigned int i, float value) {
00064     if(!ensureValidJoint(i))
00065       return;
00066     headTargets[i]=clipAngularRange(HeadOffset+i,value);
00067     markDirty();
00068   }
00069   
00070   //! 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
00071   /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) */
00072   float getJointValue(unsigned int i) const {
00073     if(ensureValidJoint(i))
00074       return headTargets[i];
00075     else
00076       return 0;
00077   }
00078   
00079   //! Centers the camera on a point in space, attempting to keep the camera as far away from the point as possible
00080   /*! Point should be relative to the body reference frame (see ::BaseFrameOffset)
00081    *  @param x location in millimeters
00082    *  @param y location in millimeters
00083    *  @param z location in millimeters
00084    *
00085    *  @todo this method is an approximation, could be more precise, and perhaps faster, although this is pretty good. */
00086   bool lookAtPoint(float x, float y, float z);
00087   
00088   //! Centers the camera on a point in space, attempting to move the camera @a d millimeters away from the point
00089   /*! Point should be relative to the body reference frame (see ::BaseFrameOffset)
00090    *  @param x location in millimeters
00091    *  @param y location in millimeters
00092    *  @param z location in millimeters
00093    *  @param d target distance from point in millimeters */
00094   bool lookAtPoint(float x, float y, float z, float d);
00095   
00096   //! Points the camera in a given direction
00097   /*! Vector should be relative to the body reference frame (see ::BaseFrameOffset)
00098    *  @param x component of the direction vector
00099    *  @param y component of the direction vector
00100    *  @param z component of the direction vector */
00101   bool lookInDirection(float x, float y, float z);
00102   
00103   //@}
00104   
00105   
00106   //!@name Inherited:
00107   virtual int updateOutputs(); //!< Updates where the head is looking
00108   virtual int isDirty() { return (dirty || !targetReached)?3:0; } //!< true if a change has been made since the last updateJointCmds() and we're active
00109   virtual int isAlive(); //!< Alive while target is not reached
00110   virtual void DoStart() { MotionCommand::DoStart(); markDirty(); } //!< marks this as dirty each time it is added
00111   //@}
00112 
00113  protected:
00114   //! puts x in the range (-pi,pi)
00115   static float normalizeAngle(float x) { return x-rint(x/(2*M_PI))*(2*M_PI); }
00116   
00117   //! 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
00118   static float clipAngularRange(unsigned int i, float x) {
00119     float min=outputRanges[i][MinRange];
00120     float max=outputRanges[i][MaxRange];
00121     if(x<min || x>max) {
00122       float mn_dist=fabs(normalizeAngle(min-x));
00123       float mx_dist=fabs(normalizeAngle(max-x));
00124       if(mn_dist<mx_dist)
00125         return min;
00126       else
00127         return max;
00128     } else
00129       return x;
00130   }
00131   //! if targetReached, reassigns headCmds from MotionManager::getOutputCmd(), then sets dirty to true and targetReached to false
00132   /*! should be called each time a joint value gets modified in case
00133    *  the head isn't where it's supposed to be, it won't jerk around
00134    * 
00135    *  MotionManager::getOutputCmd() is called instead of
00136    *  WorldState::outputs[] because if this is being called rapidly
00137    *  (i.e. after every sensor reading) using the sensor values will
00138    *  cause problems with very slow acceleration due to sensor lag
00139    *  continually resetting the current position.  Using the last
00140    *  value sent by the MotionManager fixes this.*/
00141   void markDirty();
00142 
00143   //! 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.
00144   /*! @param[in] i joint offset relative to either HeadOffset (i.e. one of TPROffset_t) or 0
00145    *  @param[out] i joint offset relative to HeadOffset (i.e. one of TPROffset_t)
00146    *  @return true if the intended joint could be ascertained, false otherwise */
00147   static bool ensureValidJoint(unsigned int& i);
00148 
00149   bool dirty;                          //!< true if a change has been made since last call to updateJointCmds()
00150   bool  hold;                          //!< if set to true, the posture will be kept active; otherwise joints will be marked unused after each posture is achieved (as if the posture was pruned); set through setHold()
00151   float tolerance;                     //!< when autopruning, if the maxdiff() of this posture and the robot's current position is below this value, isAlive() will be false, defaults to 0.01 (5.7 degree error)
00152   bool targetReached;                  //!< false if the head is still moving towards its target
00153   unsigned int targetTimestamp;        //!< time at which the targetReached flag was set
00154   unsigned int timeout;                //!< number of milliseconds to wait before giving up on a target that should have already been reached, a value of -1U will try forever
00155   float headTargets[NumHeadJoints];    //!< stores the target value of each joint
00156   OutputCmd headCmds[NumHeadJoints];   //!< stores the last values we sent from updateOutputs
00157   float maxSpeed[NumHeadJoints];       //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame
00158   ROBOOP::Robot headkin;               //!< provides kinematics computations
00159 };
00160 
00161 /*! @file
00162  * @brief Describes HeadPointerMC, a class for various ways to control where the head is looking
00163  * @author ejt (Creator)
00164  *
00165  * $Author: ejt $
00166  * $Name: tekkotsu-2_4_1 $
00167  * $Revision: 1.20 $
00168  * $State: Exp $
00169  * $Date: 2005/01/07 21:13:31 $
00170  */
00171 
00172 #endif
00173 

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