Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
HeadPointerMC.hGo 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 |